diff --git a/moon/__init__.py b/moon/__init__.py deleted file mode 100644 index 88097618f..000000000 --- a/moon/__init__.py +++ /dev/null @@ -1,4 +0,0 @@ - - -from osgar.lib import create_load_tests -load_tests = create_load_tests(__file__) diff --git a/moon/artifacts.py b/moon/artifacts.py deleted file mode 100644 index a181b545c..000000000 --- a/moon/artifacts.py +++ /dev/null @@ -1,369 +0,0 @@ -""" - Detect Cube Sat and Processing Plant artifacts -""" -from datetime import timedelta -import os -from io import StringIO -from statistics import median - -import cv2 -import numpy as np -from pathlib import Path - -from osgar.node import Node -from osgar.bus import BusShutdownException -from moon.moonnode import CAMERA_WIDTH, CAMERA_HEIGHT, CAMERA_FOCAL_LENGTH - -curdir = Path(__file__).parent - -def union(a,b): - x = min(a[0], b[0]) - y = min(a[1], b[1]) - w = max(a[0]+a[2], b[0]+b[2]) - x - h = max(a[1]+a[3], b[1]+b[3]) - y - return (x, y, w, h) - -class ArtifactDetector(Node): - def __init__(self, config, bus): - super().__init__(config, bus) - bus.register("artf", "dropped") - self.verbose = False - self.dump_dir = None # optional debug ouput into directory - self.scan = None # should laster initialize super() - self.depth = None # more precise definiton of depth image - self.width = None # detect from incoming images - self.look_for_artefacts = config.get('artefacts', []) - self.estimate_distance = config.get('estimate_distance', False) - - window_size = 5 - min_disp = 16 - num_disp = 192-min_disp - blockSize = window_size - uniquenessRatio = 7 - speckleRange = 3 - speckleWindowSize = 75 - disp12MaxDiff = 200 - P1 = 8*3*window_size**2 - P2 = 32 * 3 * window_size ** 2 - self.stereo_calc = cv2.StereoSGBM_create( - minDisparity = min_disp, - numDisparities = num_disp, - blockSize = window_size, - uniquenessRatio = uniquenessRatio, - speckleRange = speckleRange, - speckleWindowSize = speckleWindowSize, - disp12MaxDiff = disp12MaxDiff, - P1 = P1, - P2 = P2 - ) - self.Q = np.float32([[1, 0, 0, -0.5*CAMERA_WIDTH], - [0,-1, 0, 0.5*CAMERA_HEIGHT], # turn points 180 deg around x-axis, - [0, 0, 0, CAMERA_FOCAL_LENGTH], # so that y-axis looks up - [0, 0, 1/0.42, 0]]) - - self.detectors = [ - { - 'artefact_name': 'cubesat', - 'detector_type': 'classifier', - 'classifier': cv2.CascadeClassifier(str(curdir/'xml/cubesat.xml')), - 'min_size': 5, - 'max_size': 110, - 'subsequent_detects_required': 3 - }, - { - 'artefact_name': 'homebase', - 'detector_type': 'classifier', - 'classifier': cv2.CascadeClassifier(str(curdir/'xml/homebase.xml')), - 'min_size': 20, - 'max_size': 400, - 'subsequent_detects_required': 3 - }, - { - 'artefact_name': 'basemarker', - 'detector_type': 'colormatch', - 'min_size': 10, - 'max_size': 500, - 'mask': [CAMERA_HEIGHT//2, CAMERA_HEIGHT, 0, CAMERA_WIDTH], # [Y,X] order, look only in lower half of the screen (avoid solar panels) - 'pixel_count_threshold': 100, - 'bbox_union_count': 1, - 'hue_max_difference': 10, - 'hue_match': 100, # from RGB 007DBD - 'subsequent_detects_required': 3 # noise will add some of this color, wait for a consistent sequence - }, - { - 'artefact_name': 'homebase', - 'detector_type': 'colormatch', - 'min_size': 20, - 'max_size': 700, - 'mask': None, - 'pixel_count_threshold': 400, - 'bbox_union_count': 5, - 'hue_max_difference': 10, - 'hue_match': 19, # from RGB FFA616 - 'subsequent_detects_required': 3 - }, - { - 'artefact_name': 'rover', - 'detector_type': 'colormatch', - 'min_size': 10, - 'max_size': 700, - 'mask': [180, CAMERA_HEIGHT, 0, CAMERA_WIDTH], # [Y,X] order - only look in lower half of screen - 'pixel_count_threshold': 150, - 'bbox_union_count': 10, - 'hue_max_difference': 3, - 'hue_match': 27, # from RGB FFA616 - 'subsequent_detects_required': 1 - }, - { - 'artefact_name': 'excavator_arm', - 'detector_type': 'colormatch', - 'min_size': 10, - 'max_size': 200, - 'mask': [0, 120, 0, CAMERA_WIDTH], # [Y,X] order - 'pixel_count_threshold': 150, - 'bbox_union_count': 3, - 'hue_max_difference': 3, - 'hue_match': 27, # from RGB FFA616 - 'subsequent_detects_required': 1 - } - ] - self.detect_sequences = {} - - def stdout(self, *args, **kwargs): - # maybe refactor to Node? - output = StringIO() - print(*args, file=output, **kwargs) - contents = output.getvalue().strip() - output.close() -# self.publish('stdout', contents) - print(contents) - - def waitForImage(self): - self.left_image = self.right_image = None - while self.left_image is None or self.right_image is None: - self.time, channel, data = self.listen() - if channel == "left_image": - self.left_image = data - elif channel == "right_image": - self.right_image = data - return self.time - - def run(self): - try: - dropped = 0 - while True: - now = self.publish("dropped", dropped) - dropped = -1 - timestamp = now - while timestamp <= now: - # this thread is always running but wait and drop images if simulation is slower - timestamp = self.waitForImage() - dropped += 1 - self.detect_and_publish(self.left_image, self.right_image) - except BusShutdownException: - pass - - def detect_and_publish(self, left_image, right_image): - results = self.detect(left_image, right_image) - for r in results: - self.publish('artf', r) - - def detect(self, left_image, right_image): - results = [] - - limg = cv2.imdecode(np.frombuffer(left_image, dtype=np.uint8), cv2.IMREAD_COLOR) - rimg = cv2.imdecode(np.frombuffer(right_image, dtype=np.uint8), cv2.IMREAD_COLOR) - - if self.width is None: - self.stdout('Image resolution', limg.shape) - self.width = limg.shape[1] - assert self.width == limg.shape[1], (self.width, limg.shape[1]) - - - def box_area(b): - return b[2]*b[3] - - limg_rgb = cv2.cvtColor(limg, cv2.COLOR_BGR2RGB) - rimg_rgb = cv2.cvtColor(rimg, cv2.COLOR_BGR2RGB) - hsv = cv2.cvtColor(limg, cv2.COLOR_BGR2HSV) - hsv_blurred = cv2.medianBlur(hsv,5) # some frames have noise, need to blur otherwise threshold doesn't work - - objects_detected = [] - for c in self.detectors: - if c['artefact_name'] not in self.look_for_artefacts: - continue - - if c['artefact_name'] not in self.detect_sequences: - self.detect_sequences[c['artefact_name']] = 0 - - if c['detector_type'] == 'colormatch': - lower_hue = np.array([c['hue_match'] - c['hue_max_difference'],50,50]) - upper_hue = np.array([c['hue_match'] + c['hue_max_difference'],255,255]) - # Threshold the HSV image to get only the matching colors - mask = cv2.inRange(hsv_blurred, lower_hue, upper_hue) - if c['mask'] is not None: - m = np.zeros([CAMERA_HEIGHT,CAMERA_WIDTH], dtype=np.uint8) - m[c['mask'][0]:c['mask'][1],c['mask'][2]:c['mask'][3]] = 255 - mask &= m - - bboxes = [] - contours = cv2.findContours(mask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) - contours = contours[0] if len(contours) == 2 else contours[1] - for cont in contours: - contours_poly = cv2.approxPolyDP(cont, 3, True) - x,y,w,h = cv2.boundingRect(contours_poly) - if w > 1 or h > 1: # ignore isolated pixels - bboxes.append([int(x),int(y),int(w),int(h)]) - - if len(bboxes) > 0: - sb = sorted(bboxes, key = box_area, reverse = True)[:c['bbox_union_count']] - bbox = sb[0] - for b in sb[1:]: - bbox = union(bbox,b) - x, y, w, h = bbox - match_count = cv2.countNonZero(mask[y:y+h,x:x+w]) - if ( - match_count > c['pixel_count_threshold'] and - w >= c['min_size'] and h >= c['min_size'] and - w <= c['max_size'] and h <= c['max_size'] - ): - # print ("%s match count: %d; [%d %d %d %d]" % (c['artefact_name'], match_count, x, y, w, h)) - objects_detected.append(c['artefact_name']) - if self.detect_sequences[c['artefact_name']] < c['subsequent_detects_required']: - # do not act until you have detections in a row - self.detect_sequences[c['artefact_name']] += 1 - else: - if self.estimate_distance: - disp = self.stereo_calc.compute(limg_rgb, rimg_rgb).astype(np.float32) / 16.0 - points = cv2.reprojectImageTo3D(disp, self.Q) - matching_points = points[mask != 0] - distances = matching_points[:,2] # third column are Z coords (distances) - - mean = np.mean(distances) - sd = np.std(distances) - distances_clean = [x for x in distances if mean - 2 * sd < x < mean + 2 * sd] - - #print("Artf distance: min %.1f median: %.1f" % (min(distances), median(distances))) - if len(distances_clean) == 0: - distances_clean = distances - # print("Artf cleaned: min %.1f median: %.1f" % (min(final_list), median(final_list))) - dist = max(0.0, min(distances_clean)) # subtract about half length of the rover - else: - dist = 0.0 - results.append((c['artefact_name'], int(x), int(y), int(w), int(h), int(match_count), float(dist))) - - if c['detector_type'] == 'classifier': - lfound = c['classifier'].detectMultiScale(limg_rgb, minSize =(c['min_size'], c['min_size']), maxSize =(c['max_size'], c['max_size'])) - rfound = c['classifier'].detectMultiScale(rimg_rgb, minSize =(c['min_size'], c['min_size']), maxSize =(c['max_size'], c['max_size'])) - - if len(lfound) > 0 and len(rfound) > 0: # only report if both cameras see it - objects_detected.append(c['artefact_name']) - if self.detect_sequences[c['artefact_name']] < c['subsequent_detects_required']: # do not act until you have detections in a row - self.detect_sequences[c['artefact_name']] += 1 - else: - - # TODO: tweak the filtering (blur and threshold), sometimes not all background is filtered out and the bbox looks bigger than it should be - x,y,width,height = lfound[0] - # print(self.time, "Pre: %d %d %d %d" % (x,y,width,height)) - gray = cv2.cvtColor(limg_rgb[y:y+height, x:x+width], cv2.COLOR_BGR2GRAY) - blur = cv2.medianBlur(gray,3) # some frames have noise, need to blur otherwise threshold doesn't work - th, threshed = cv2.threshold(blur, 30, 255, cv2.THRESH_BINARY) - coords = cv2.findNonZero(threshed) - nonzerocount = cv2.countNonZero(threshed) - nx, ny, nw, nh = cv2.boundingRect(coords) - # print(self.time, "Post: %d %d %d %d" % (x+nx,y+ny,nw,nh)) - - results.append((c['artefact_name'], int(x+nx), int(y+ny), int(nw), int(nh), int(nonzerocount))) - - for artefact_name in self.detect_sequences.keys(): - if artefact_name not in objects_detected: - self.detect_sequences[artefact_name] = 0 - - return results - - -def debug2dir(filename, out_dir): - from osgar.logger import LogReader, lookup_stream_names - from osgar.lib.serialize import deserialize - - names = lookup_stream_names(filename) - assert 'detector.debug_artf' in names, names - assert 'detector.artf' in names, names - assert 'rosmsg.sim_time_sec' in names, names - image_id = names.index('detector.debug_artf') + 1 - artf_id = names.index('detector.artf') + 1 - sim_sec_id = names.index('rosmsg.sim_time_sec') + 1 - sim_time_sec = None - image = None - artf = None - for dt, channel, data in LogReader(filename, only_stream_id=[image_id, artf_id, sim_sec_id]): - data = deserialize(data) - if channel == sim_sec_id: - sim_time_sec = data - elif channel == image_id: - image = data - assert artf is not None - time_sec = sim_time_sec if sim_time_sec is not None else int(dt.total_seconds()) - name = os.path.basename(filename)[:-4] + '-' + artf[0] + '-' + str(time_sec) + '.jpg' - print(name) - with open(os.path.join(out_dir, name), 'wb') as f: - f.write(image) - elif channel == artf_id: - artf = data - - -if __name__ == '__main__': - from unittest.mock import MagicMock - from queue import Queue - import argparse - import datetime - import sys - from osgar.bus import Bus - - parser = argparse.ArgumentParser(description='Run artifact detection and classification for given JPEG image') - parser.add_argument('filename', help='JPEG filename') - parser.add_argument('--debug2dir', help='dump clasified debug images into directory') - parser.add_argument('-v', '--verbose', help='verbose mode', action='store_true') - args = parser.parse_args() - - if args.debug2dir is not None: - debug2dir(args.filename, args.debug2dir) - sys.exit() - - with open(args.filename.replace('.npz', '.jpg'), 'rb') as f: - jpeg_data = f.read() - - config = {'virtual_world': True} # for now - logger = MagicMock() - logger.register = MagicMock(return_value=1) - def counter(): - start = datetime.datetime.utcnow() - while True: - dt = datetime.datetime.utcnow() - start - yield dt - logger.write = MagicMock(side_effect=counter()) - bus = Bus(logger) - detector = ArtifactDetector(config, bus.handle('detector')) - detector.verbose = args.verbose - tester = bus.handle('tester') - tester.register('scan', 'left_image', 'right_image', 'tick') - bus.connect('tester.scan', 'detector.scan') - bus.connect('tester.left_image', 'detector.left_image') - bus.connect('tester.right_image', 'detector.right_image') - bus.connect('detector.artf', 'tester.artf') - bus.connect('tester.tick', 'tester.tick') - bus.connect('detector.dropped', 'tester.dropped') - tester.publish('scan', [2000]*270) # pretend that everything is at 2 meters - detector.start() - for i in range(10 + 1): # workaround for local minima - a = tester.listen() -# print(i, a) - tester.sleep(0.01) - tester.publish('left_image', jpeg_data) # TODO right image - detector.request_stop() - detector.join() - tester.publish('tick', None) - a = tester.listen() - print(a) - -# vim: expandtab sw=4 ts=4 diff --git a/moon/bulk_test.sh b/moon/bulk_test.sh deleted file mode 100755 index 213aa4340..000000000 --- a/moon/bulk_test.sh +++ /dev/null @@ -1,166 +0,0 @@ -#!/bin/bash - -# NASA qualification round number -ROUND=3 - -# after how long should the simulation give up (in seconds); note the round is allowed to run for up to 45 minutes -TIMEOUT=1800 - -# desired score -SCORE="13" - -RANGE_START=0 -RANGE_END=49 - -SRCP2_WAIT=15 - -UNSUCCESS_LOGS_ONLY=0 - -LOG_DIR_PREFIX="." - -REPEAT_SEED=1 - -VSLAM=0 - -while getopts "r:t:s:f:l:hw:ud:p:v" opt; do - case ${opt} in - h) - echo "Usage:" - echo " -r " - echo " -t " - echo " -s " - echo " -f " - echo " -l " - echo " -w " - echo " -u .. only record unsuccessful run logs" - echo " -d " - echo " -p " - echo " -v " - exit 0 - ;; - r) - ROUND=$OPTARG - ;; - t) - TIMEOUT=$(expr 60 \* $OPTARG) - ;; - s) - SCORE=$OPTARG - ;; - f) - RANGE_START=$OPTARG - ;; - l) - RANGE_END=$OPTARG - ;; - w) - SRCP2_WAIT=$OPTARG - ;; - u) - UNSUCCESS_LOGS_ONLY=1 - ;; - d) - LOG_DIR_PREFIX=$OPTARG - ;; - p) - REPEAT_SEED=$OPTARG - ;; - v) - VSLAM=1 - ;; - esac -done - -if (docker ps | grep -q rover); then - echo "OSGAR docker already running, exiting" - exit 1 -fi - -if (docker ps | grep -q scheducation); then - echo "SRCP2 docker already running, exiting" - exit 1 -fi - -SUCCESS_STRING="Score: ${SCORE}" -case ${ROUND} in - 1) - FAIL_STRING="Sleep and finish" - FAIL_SWITCH="-e \"${FAIL_STRING}\"" - ;; - 2) - FAIL_STRING="Sleep and finish" - FAIL_SWITCH="-e \"${FAIL_STRING}\"" - ;; - 3) - FAIL_STRING="object location incorrect" - FAIL_SWITCH="-e \"${FAIL_STRING}\"" - ;; -esac - -now=$(date +"%m%d%Y_%H%M") -log_dir="${LOG_DIR_PREFIX}/bulklogs_${now}" -mkdir "${log_dir}" - -echo "Round: ${ROUND}, Score: ${SCORE}; Fail String: ${FAIL_STRING}; Seed range: ${RANGE_START}-${RANGE_END}; Repeat seeds: ${REPEAT_SEED}; Timeout(min): `expr ${TIMEOUT} / 60`; Unsuccessful logs only: ${UNSUCCESS_LOGS_ONLY}; Log dir: \"${log_dir}\"" - -for i in $(seq $RANGE_START $RANGE_END) -do - - for j in $(seq 1 $REPEAT_SEED) - do - - log_file="${log_dir}"/osgar-tty-${now}-seed_${i}_run_${j}.txt - - echo "==========" `date` "====== seed:" $i " run: " $j - $HOME/space-challenge/srcp2-competitors/docker/scripts/launch/roslaunch_docker -q -n --run-round $ROUND -s ${i} >& /dev/null - - # wait for SRCP2 simulator to start - sleep ${SRCP2_WAIT} - - if [[ $VSLAM -eq 1 ]]; then - if [[ $ROUND -eq 2 ]]; then - docker run --init --network=host --rm --name openvslam1 -t openvslam-rospublish -r excavator_1 >& /dev/null & - docker run --init --network=host --rm --name openvslam2 -t openvslam-rospublish -r hauler_1 >& /dev/null & - vslam1_id=`docker image ls | grep openvslam1 | head -1 | awk '{print $3}'` - vslam2_id=`docker image ls | grep openvslam2 | head -1 | awk '{print $3}'` - echo "Running OpenVSLAM images ${vslam1_id} and ${vslam1_id}" >> "${log_file}" - else - docker run --init --network=host --rm --name openvslam -t openvslam-rospublish >& /dev/null & - vslam_id=`docker image ls | grep openvslam | head -1 | awk '{print $3}'` - echo "Running OpenVSLAM image ${vslam_id}" >> "${log_file}" - fi - fi - - osgar_image_id=`docker image ls | grep rover | grep latest | awk '{print $3}'` - srcp2_image_id=`docker image ls | grep comp | head -1 | awk '{print $3}'` - echo "Running OSGAR image ${osgar_image_id}" >> "${log_file}" - echo "Running SRCP2 image ${srcp2_image_id}" >> "${log_file}" - - timeout $TIMEOUT sh -c "( docker run --rm --network=host -t --name nasa-rover rover:latest /osgar/moon/docker/rover/run_solution.bash -u -r $ROUND &) 2>&1 | tee -a \"${log_file}\" | grep -m 1 ${FAIL_SWITCH} -e \"${SUCCESS_STRING}\"" - # timeout exit status 124 means the timeout limit was reached - timeout_status=$? - grep -q "${SUCCESS_STRING}" < "${log_file}" - was_found=$? - if [[ $timeout_status -eq 124 || $was_found -ne 0 || $UNSUCCESS_LOGS_ONLY -eq 0 ]]; then - for f in $(docker exec -it nasa-rover /bin/bash -c "ls /*.log"); do - f=`echo $f | sed 's/\r//g'` - readarray -d . -t strarr <<< "$f" - docker cp nasa-rover:$f "${log_dir}"/${strarr[0]}-seed_${i}_run_${j}.olog - done - fi - - docker kill nasa-rover >& /dev/null - - if [[ $VSLAM -eq 1 ]]; then - if [[ $ROUND -eq 2 ]]; then - docker kill openvslam1 >& /dev/null - docker kill openvslam2 >& /dev/null - else - docker kill openvslam >& /dev/null - fi - fi - - docker kill srcp2-simulation >& /dev/null - sleep ${SRCP2_WAIT} - done -done diff --git a/moon/config/moon-excavator-round2.json b/moon/config/moon-excavator-round2.json deleted file mode 100644 index f4a87e69e..000000000 --- a/moon/config/moon-excavator-round2.json +++ /dev/null @@ -1,114 +0,0 @@ -{ - "version": 2, - "robot": { - "modules": { - "app": { - "driver": "moon.controller_excavator_round2:SpaceRoboticsChallengeExcavatorRound2", - "in": ["odo_pose"], - "out": ["desired_speed", "artf_cmd", "request_origin", "bucket_cmd"], - "init": { - "debug": true - } - }, - "transmitter": { - "driver": "zeromq", - "in": [], - "out": ["raw"], - "init": { - "mode": "PUSH", - "endpoint": "tcp://localhost:5556" - } - }, - "receiver": { - "driver": "zeromq", - "in": [], - "out": ["raw"], - "init": { - "mode": "PULL", - "endpoint": "tcp://localhost:5555" - } - }, - "rpc": { - "driver": "zeromq", - "in": ["request"], - "out": ["response"], - "init": { - "mode": "REQ", - "endpoint": "tcp://localhost:5557", - "save_data": true - } - }, - "rosmsg": { - "driver": "rosmsg", - "in": ["raw", "desired_speed", "tick", "stdout"], - "out": ["rot", "acc", "scan", "image", "sim_clock", "cmd", "origin"], - "init": { - "topics": [ - ["/excavator_1/bucket_info", "srcp2_msgs/ExcavatorMsg"], - ["/excavator_1/joint_states", "sensor_msgs/JointState"], - ["/qual_2_score", "srcp2_msgs/Qual2ScoringMsg"], - ["/excavator_1/camera/left/image_raw/compressed", "sensor_msgs/CompressedImage", "left_image"], - ["/excavator_1/camera/right/image_raw/compressed", "sensor_msgs/CompressedImage", "right_image"], - ["/excavator_1/imu", "sensor_msgs/Imu", "orientation"], - ["/excavator_1/openvslam/pose", "geometry_msgs/PoseStamped", "vslam_pose"], - ["/excavator_1/openvslam/enabled", "std_msgs/Bool", "vslam_enabled"], - ["/osgar/broadcast", "std_msgs/String", "osgar_broadcast"], - ["/excavator_1/laser/scan", "sensor_msgs/LaserScan", "scan"] - ] - } - }, - "excavator": { - "driver": "moon.vehicles.excavator:Excavator", - "in": [], - "out": [], - "init": { - } - }, - "detector": { - "driver": "moon.artifacts:ArtifactDetector", - "in": [], - "out": [], - "init": { - "artefacts": ["rover"], - "estimate_distance": false - } - } - }, - "links": [["receiver.raw", "rosmsg.raw"], - ["rosmsg.cmd", "transmitter.raw"], - ["rosmsg.rot", "app.rot"], - ["rosmsg.rot", "excavator.rot"], - ["rosmsg.scan", "app.scan"], - ["rosmsg.osgar_broadcast", "app.osgar_broadcast"], - ["rosmsg.vslam_pose", "app.vslam_pose"], - ["rosmsg.vslam_enabled", "app.vslam_enabled"], - ["rosmsg./qual_2_score", "app.score"], - ["rosmsg.joint_name", "excavator.joint_name"], - ["rosmsg.left_image", "detector.left_image"], - ["rosmsg.left_image", "app.left_image"], - ["rosmsg.right_image", "detector.right_image"], - ["rosmsg.joint_name", "app.joint_name"], - ["rosmsg.joint_position", "app.joint_position"], - ["rosmsg.joint_position", "excavator.joint_position"], - ["rosmsg.joint_velocity", "excavator.joint_velocity"], - ["rosmsg.joint_effort", "excavator.joint_effort"], - ["rosmsg.sim_clock", "excavator.sim_clock"], - ["rosmsg.sim_clock", "app.sim_clock"], - ["rosmsg./excavator_1/bucket_info", "app.bucket_info"], - ["app.desired_speed", "excavator.desired_speed"], - ["app.desired_movement", "excavator.desired_movement"], - ["app.bucket_dig", "excavator.bucket_dig"], - ["app.bucket_drop", "excavator.bucket_drop"], - ["app.request", "rpc.request"], - ["excavator.request", "rpc.request"], - ["rpc.response", "app.response"], - ["rpc.response", "excavator.response"], - ["detector.artf", "app.artf"], - ["excavator.cmd", "transmitter.raw"], - ["app.cmd", "transmitter.raw"], - ["excavator.bucket_cmd", "transmitter.raw"], - ["excavator.odo_pose", "app.odo_pose"], - ["excavator.desired_speeds", "app.desired_speeds"] - ] - } -} diff --git a/moon/config/moon-hauler-round2.json b/moon/config/moon-hauler-round2.json deleted file mode 100644 index f71864c4d..000000000 --- a/moon/config/moon-hauler-round2.json +++ /dev/null @@ -1,108 +0,0 @@ -{ - "version": 2, - "robot": { - "modules": { - "app": { - "driver": "moon.controller_hauler_round2:SpaceRoboticsChallengeHaulerRound2", - "in": ["odo_pose"], - "out": ["desired_speed", "artf_cmd", "request_origin"], - "init": { - "debug": true - } - }, - "transmitter": { - "driver": "zeromq", - "in": [], - "out": ["raw"], - "init": { - "mode": "PUSH", - "endpoint": "tcp://localhost:6556" - } - }, - "receiver": { - "driver": "zeromq", - "in": [], - "out": ["raw"], - "init": { - "mode": "PULL", - "endpoint": "tcp://localhost:6555" - } - }, - "rpc": { - "driver": "zeromq", - "in": ["request"], - "out": ["response"], - "init": { - "mode": "REQ", - "endpoint": "tcp://localhost:6557", - "save_data": true - } - }, - "rosmsg": { - "driver": "rosmsg", - "in": ["raw", "desired_speed", "tick", "stdout"], - "out": ["rot", "acc", "scan", "image", "sim_clock", "cmd", "origin"], - "init": { - "topics": [ - ["/hauler_1/bin_info", "srcp2_msgs/HaulerMsg"], - ["/hauler_1/joint_states", "sensor_msgs/JointState"], - ["/qual_2_score", "srcp2_msgs/Qual2ScoringMsg"], - ["/hauler_1/camera/left/image_raw/compressed", "sensor_msgs/CompressedImage", "left_image"], - ["/hauler_1/camera/right/image_raw/compressed", "sensor_msgs/CompressedImage", "right_image"], - ["/hauler_1/imu", "sensor_msgs/Imu", "orientation"], - ["/hauler_1/bin_info", "srcp2_msgs/HaulerMsg", "bin_info"], - ["/hauler_1/openvslam/pose", "geometry_msgs/PoseStamped", "vslam_pose"], - ["/hauler_1/openvslam/enabled", "std_msgs/Bool", "vslam_enabled"], - ["/osgar/broadcast", "std_msgs/String", "osgar_broadcast"], - ["/hauler_1/laser/scan", "sensor_msgs/LaserScan", "scan"] - ] - } - }, - "hauler": { - "driver": "moon.vehicles.hauler:Hauler", - "in": [], - "out": [], - "init": { - } - }, - "detector": { - "driver": "moon.artifacts:ArtifactDetector", - "in": [], - "out": [], - "init": { - "artefacts": ["rover", "excavator_arm"], - "estimate_distance": true - } - } - }, - "links": [["receiver.raw", "rosmsg.raw"], - ["rosmsg.cmd", "transmitter.raw"], - ["rosmsg.rot", "app.rot"], - ["rosmsg.rot", "hauler.rot"], - ["rosmsg.scan", "app.scan"], - ["rosmsg.bin_info", "app.bin_info"], - ["rosmsg.osgar_broadcast", "app.osgar_broadcast"], - ["rosmsg.vslam_pose", "app.vslam_pose"], - ["rosmsg.vslam_enabled", "app.vslam_enabled"], - ["rosmsg./qual_2_score", "app.score"], - ["rosmsg.joint_name", "hauler.joint_name"], - ["rosmsg.joint_position", "hauler.joint_position"], - ["rosmsg.joint_velocity", "hauler.joint_velocity"], - ["rosmsg.joint_effort", "hauler.joint_effort"], - ["rosmsg.sim_clock", "hauler.sim_clock"], - ["rosmsg.sim_clock", "app.sim_clock"], - ["rosmsg.left_image", "detector.left_image"], - ["rosmsg.right_image", "detector.right_image"], - ["app.desired_speed", "hauler.desired_speed"], - ["app.desired_movement", "hauler.desired_movement"], - ["app.request", "rpc.request"], - ["hauler.request", "rpc.request"], - ["rpc.response", "app.response"], - ["detector.artf", "app.artf"], - ["hauler.cmd", "transmitter.raw"], - ["app.cmd", "transmitter.raw"], - ["hauler.odo_pose", "app.odo_pose"], - ["hauler.desired_speeds", "app.desired_speeds"] - ] - } -} diff --git a/moon/config/moon-round1.json b/moon/config/moon-round1.json deleted file mode 100644 index dbde35c96..000000000 --- a/moon/config/moon-round1.json +++ /dev/null @@ -1,105 +0,0 @@ -{ - "version": 2, - "robot": { - "modules": { - "app": { - "driver": "moon.controller_round1:SpaceRoboticsChallengeRound1", - "in": ["odo_pose", "driving_control"], - "out": ["desired_speed", "artf_xyz", "artf_cmd", "pose3d", "driving_recovery"], - "init": {"debug": false} - }, - "transmitter": { - "driver": "zeromq", - "in": [], - "out": ["raw"], - "init": { - "mode": "PUSH", - "endpoint": "tcp://localhost:5556" - } - }, - "receiver": { - "driver": "zeromq", - "in": [], - "out": ["raw"], - "init": { - "mode": "PULL", - "endpoint": "tcp://localhost:5555" - } - }, - "rpc": { - "driver": "zeromq", - "in": ["request"], - "out": ["response"], - "init": { - "mode": "REQ", - "endpoint": "tcp://localhost:5557", - "save_data": true - } - }, - "rosmsg": { - "driver": "rosmsg", - "in": ["raw", "desired_speed", "tick", "stdout", "request_origin"], - "out": ["rot", "acc", "scan", "image", "odo_pose", "sim_clock", "cmd", "origin"], - "init": { - "topics": [ - ["/scout_1/joint_states", "sensor_msgs/JointState"], - ["/qual_1_score", "srcp2_msgs/Qual1ScoringMsg"], - ["/scout_1/volatile_sensor", "srcp2_msgs/VolSensorMsg"], - ["/scout_1/camera/left/image_raw/compressed", "sensor_msgs/CompressedImage", "left_image"], - ["/scout_1/camera/right/image_raw/compressed", "sensor_msgs/CompressedImage", "right_image"], - ["/scout_1/imu", "sensor_msgs/Imu", "orientation"], - ["/scout_1/openvslam/pose", "geometry_msgs/PoseStamped", "vslam_pose"], - ["/scout_1/openvslam/enabled", "std_msgs/Bool", "vslam_enabled"], - ["/scout_1/laser/scan", "sensor_msgs/LaserScan", "scan"] - ] - } - }, - "scout": { - "driver": "moon.vehicles.scout:Scout", - "in": [], - "out": [], - "init": { - } - }, - "detector": { - "driver": "moon.artifacts:ArtifactDetector", - "in": [], - "out": [], - "init": { - "artefacts": ["homebase"] - } - } - }, - "links": [["receiver.raw", "rosmsg.raw"], - ["rosmsg.cmd", "transmitter.raw"], - ["rosmsg.rot", "app.rot"], - ["rosmsg.orientation", "app.orientation"], - ["rosmsg.scan", "app.scan"], - ["rosmsg.vslam_pose", "app.vslam_pose"], - ["rosmsg.vslam_enabled", "app.vslam_enabled"], - ["rosmsg./scout_1/volatile_sensor", "scout.volatile"], - ["rosmsg./qual_1_score", "app.score"], - ["rosmsg.joint_name", "scout.joint_name"], - ["rosmsg.sim_clock", "scout.sim_clock"], - ["rosmsg.sim_clock", "app.sim_clock"], - ["rosmsg.joint_position", "scout.joint_position"], - ["rosmsg.joint_velocity", "scout.joint_velocity"], - ["rosmsg.joint_effort", "scout.joint_effort"], - ["rosmsg.rot", "scout.rot"], - ["rosmsg.left_image", "detector.left_image"], - ["rosmsg.right_image", "detector.right_image"], - ["detector.artf", "app.artf"], - ["app.desired_speed", "scout.desired_speed"], - ["app.desired_movement", "scout.desired_movement"], - ["app.request", "rpc.request"], - ["scout.request", "rpc.request"], - ["rpc.response", "app.response"], - ["rpc.response", "scout.response"], - ["scout.cmd", "transmitter.raw"], - ["app.cmd", "transmitter.raw"], - ["scout.object_reached", "app.object_reached"], - ["scout.odo_pose", "app.odo_pose"], - ["scout.desired_speeds", "app.desired_speeds"] - ] - } -} diff --git a/moon/config/moon-round3.json b/moon/config/moon-round3.json deleted file mode 100644 index 322bd30fb..000000000 --- a/moon/config/moon-round3.json +++ /dev/null @@ -1,104 +0,0 @@ -{ - "version": 2, - "robot": { - "modules": { - "app": { - "driver": "moon.controller_round3:SpaceRoboticsChallengeRound3", - "in": ["pose2d", "driving_control", "driving_recovery"], - "out": ["desired_speed", "driving_recovery"], - "init": { - "debug": false - } - }, - "transmitter": { - "driver": "zeromq", - "in": [], - "out": ["raw"], - "init": { - "mode": "PUSH", - "endpoint": "tcp://localhost:5556" - } - }, - "receiver": { - "driver": "zeromq", - "in": [], - "out": ["raw"], - "init": { - "mode": "PULL", - "endpoint": "tcp://localhost:5555" - } - }, - "rpc": { - "driver": "zeromq", - "in": ["request"], - "out": ["response"], - "init": { - "mode": "REQ", - "endpoint": "tcp://localhost:5557", - "save_data": true - } - }, - "rosmsg": { - "driver": "rosmsg", - "in": ["raw", "desired_speed", "tick", "stdout"], - "out": ["rot", "acc", "scan", "image", "sim_clock", "cmd"], - "init": { - "topics": [ - ["/scout_1/joint_states", "sensor_msgs/JointState"], - ["/qual_3_score", "srcp2_msgs/Qual3ScoringMsg"], - ["/scout_1/camera/left/image_raw/compressed", "sensor_msgs/CompressedImage", "left_image"], - ["/scout_1/camera/right/image_raw/compressed", "sensor_msgs/CompressedImage", "right_image"], - ["/scout_1/imu", "sensor_msgs/Imu", "orientation"], - ["/scout_1/openvslam/pose", "geometry_msgs/PoseStamped", "vslam_pose"], - ["/scout_1/laser/scan", "sensor_msgs/LaserScan", "scan"] - ] - } - }, - "scout": { - "driver": "moon.vehicles.scout:Scout", - "in": [], - "out": [], - "init": { - } - }, - "detector": { - "driver": "moon.artifacts:ArtifactDetector", - "in": [], - "out": [], - "init": { - "artefacts": ["homebase", "cubesat", "basemarker"] - } - } - }, - "links": [ - ["receiver.raw", "rosmsg.raw"], - ["rosmsg.cmd", "transmitter.raw"], - ["rosmsg.rot", "app.rot"], - ["rosmsg.scan", "app.scan"], - ["rosmsg.vslam_pose", "app.vslam_pose"], - ["rosmsg./qual_3_score", "app.score"], - ["rosmsg.joint_name", "scout.joint_name"], - ["rosmsg.joint_name", "app.joint_name"], - ["rosmsg.joint_position", "scout.joint_position"], - ["rosmsg.joint_position", "app.joint_position"], - ["rosmsg.joint_velocity", "scout.joint_velocity"], - ["rosmsg.joint_effort", "scout.joint_effort"], - ["rosmsg.sim_clock", "scout.sim_clock"], - ["rosmsg.sim_clock", "app.sim_clock"], - ["rosmsg.rot", "scout.rot"], - ["rosmsg.left_image", "detector.left_image"], - ["rosmsg.right_image", "detector.right_image"], - ["detector.artf", "app.artf"], - ["app.desired_movement", "scout.desired_movement"], - ["app.desired_speed", "scout.desired_speed"], - ["app.driving_recovery", "scout.driving_recovery"], - ["app.driving_recovery", "app.driving_recovery"], - ["app.request", "rpc.request"], - ["rpc.response", "app.response"], - ["scout.cmd", "transmitter.raw"], - ["app.cmd", "transmitter.raw"], - ["scout.odo_pose", "app.odo_pose"], - ["scout.desired_speeds", "app.desired_speeds"] - ] - } -} diff --git a/moon/controller.py b/moon/controller.py deleted file mode 100644 index 193c05abc..000000000 --- a/moon/controller.py +++ /dev/null @@ -1,850 +0,0 @@ -""" - Space Robotics Challenge 2 -""" -import math -from math import sqrt -import numpy as np -import io - -from datetime import timedelta -from statistics import median -from shapely.geometry import Point, LineString -from shapely.geometry.polygon import Polygon - -from osgar.bus import BusShutdownException -from osgar.lib import quaternion -from osgar.lib.quaternion import euler_zyx, euler_to_quaternion - -from osgar.lib.mathex import normalizeAnglePIPI -from osgar.lib.virtual_bumper import VirtualBumper - -from moon.moonnode import MoonNode, LIDAR_BEAM_SPACING - -TURN_RADIUS = 8 # radius of circle when turning -AVOID_RADIUS = 4 # radius to use when going around an obstacle (this means it will not rush to go back to the same direction once it disappears off lidar) -GO_STRAIGHT = float("inf") -AVOIDANCE_DURATION = 3000 # milliseconds -AVOIDANCE_TURN_DURATION = 800 -VIRTUAL_FENCE_LOOK_AHEAD_DISTANCE = 6 # max distance in front of the rover to investigate overlap with unsafe regions for - -class MoonException(Exception): - pass -class ChangeDriverException(MoonException): - pass - -class VSLAMLostException(MoonException): - pass - -class VSLAMDisabledException(MoonException): - pass - -class VSLAMEnabledException(MoonException): - pass - -class VSLAMFoundException(MoonException): - pass - -class VirtualBumperException(MoonException): - pass - -class LidarCollisionException(MoonException): - pass - -def pol2cart(rho, phi): - x = rho * math.cos(phi) - y = rho * math.sin(phi) - return(x, y) - -def cart2pol(x, y): - rho = np.sqrt(x**2 + y**2) - phi = np.arctan2(y, x) - return(rho, phi) - -def best_fit_circle(x_l, y_l): - # Best Fit Circle https://goodcalculators.com/best-fit-circle-least-squares-calculator/ - # receive 180 scan samples, first and last 40 are discarded, remaining 100 samples represent 2.6rad view - # samples are supposed to form a circle which this routine calculates - - nop = len(x_l) - x = np.array(x_l) - y = np.array(y_l) - - x_y = np.multiply(x,y) - x_2 = np.square(x) - y_2 = np.square(y) - - x_2_plus_y_2 = np.add(x_2,y_2) - x__x_2_plus_y_2 = np.multiply(x,x_2_plus_y_2) - y__x_2_plus_y_2 = np.multiply(y,x_2_plus_y_2) - - sum_x = x.sum(dtype=float) - sum_y = y.sum(dtype=float) - sum_x_2 = x_2.sum(dtype=float) - sum_y_2 = y_2.sum(dtype=float) - sum_x_y = x_y.sum(dtype=float) - sum_x_2_plus_y_2 = x_2_plus_y_2.sum(dtype=float) - sum_x__x_2_plus_y_2 = x__x_2_plus_y_2.sum(dtype=float) - sum_y__x_2_plus_y_2 = y__x_2_plus_y_2.sum(dtype=float) - - m3b3 = np.array([[sum_x_2,sum_x_y,sum_x], - [sum_x_y,sum_y_2,sum_y], - [sum_x,sum_y,nop]]) - invm3b3 = np.linalg.inv(m3b3) - m3b1 = np.array([sum_x__x_2_plus_y_2,sum_y__x_2_plus_y_2,sum_x_2_plus_y_2]) - A=np.dot(invm3b3,m3b1)[0] - B=np.dot(invm3b3,m3b1)[1] - C=np.dot(invm3b3,m3b1)[2] - homebase_cx = A/2 - homebase_cy = B/2 - homebase_radius = np.sqrt(4*C+A**2+B**2)/2 - - return(homebase_cx, homebase_cy, homebase_radius) - - -def eulerAnglesToRotationMatrix(theta) : - # TODO: correctness of signs in R_x and R_y has not been verified - R_x = np.array([[1, 0, 0, 0 ], - [0, math.cos(theta[0]), math.sin(theta[0]), 0 ], - [0, -math.sin(theta[0]), math.cos(theta[0]), 0 ], - [0, 0, 0, 1 ] - ]) - - - - R_y = np.array([[math.cos(theta[1]), 0, -math.sin(theta[1]), 0 ], - [0, 1, 0, 0 ], - [math.sin(theta[1]), 0, math.cos(theta[1]), 0 ], - [0, 0, 0, 1 ] - ]) - - R_z = np.array([[math.cos(theta[2]), -math.sin(theta[2]), 0, 0], - [math.sin(theta[2]), math.cos(theta[2]), 0, 0], - [0, 0, 1, 0], - [0, 0, 0, 1] - ]) - - - R = np.dot(R_z, np.dot( R_y, R_x )) - - return R - -def translationToMatrix(v): - return np.asmatrix(np.array( - [[1, 0, 0, v[0]], - [0, 1, 0, v[1]], - [0, 0, 1, v[2]], - [0,0,0,1]] - )) - -def distance(pose1, pose2): - return math.hypot(pose1[0] - pose2[0], pose1[1] - pose2[1]) - -def calc_tangents(C,r,P): - dx, dy = P[0]-C[0], P[1]-C[1] - dxr, dyr = -dy, dx - d = sqrt(dx**2+dy**2) - if d >= r : - rho = r/d - ad = rho**2 - bd = rho*sqrt(1-rho**2) - T1 = [C[0] + ad*dx + bd*dxr, C[1] + ad*dy + bd*dyr] - T2 = [C[0] + ad*dx - bd*dxr, C[1] + ad*dy - bd*dyr] - - if (d/r-1) < 1E-8: - print('controller.calc_tangents: P is on the circumference') - return [T1, T2] - print('controller.calc_tangents: P inside circle') - - -def min_dist(laser_data): - if len(laser_data) > 0: - # remove ultra near reflections and unlimited values == 0 - laser_data = [x if x > 10 else 10000 for x in laser_data] - return min(laser_data)/1000.0 - return 0 - - -class LidarCollisionMonitor: - def __init__(self, robot, threshold_distance=1200): - self.robot = robot - self.scan_history = [] - self.threshold_distance = threshold_distance #1.2m - self.min_hits = 10 # we have to see at least 10 points nearer than threshold - - def update(self, robot, channel): - if channel == 'scan': - # measure distance only in 66 degree angle (about the width of the robot 1.5m ahead) - # NASA Lidar 150degrees wide, 100 samples - # robot is ~2.21m wide (~1.2m x 2 with wiggle room) - # TODO: rework using lidar processing from main class - - collision_view = robot.scan[70:110] - self.scan_history.append(collision_view) - if len(self.scan_history) > 3: - self.scan_history.pop(0) - - median_scan = [] - for j in range(len(collision_view)): - median_scan.append(median([self.scan_history[i][j] for i in range(len(self.scan_history))])) - - iterator = filter(lambda dist : 10 < dist < self.threshold_distance, median_scan) - if len(list(iterator)) >= self.min_hits and not robot.inException: - robot.publish('driving_recovery', True) - raise LidarCollisionException() - - # context manager functions - def __enter__(self): - self.callback = self.robot.register(self.update) - return self - - def __exit__(self, exc_type, exc_val, exc_tb): - self.robot.unregister(self.callback) - -def ps(*args, **kwargs): - output = io.StringIO() - print(*args, file=output, **kwargs) - contents = output.getvalue() - output.close() - return contents - -class SpaceRoboticsChallenge(MoonNode): - def __init__(self, config, bus): - super().__init__(config, bus) - bus.register("desired_speed", "desired_movement", "driving_recovery", "cmd", "pose2d") - self.robot_name = "scout_1" - - self.joint_name = None - self.sensor_joint_position = None - - self.last_position = None # 2D pose (x, y, heading) in rover coordinates, used for local operations (go 10m) - self.max_speed = 1.0 # oficial max speed is 1.5m/s - self.max_angular_speed = math.radians(60) - - # best (fusion) values - self.yaw, self.pitch, self.roll = 0, 0, 0 - self.xyz = None - self.xyz_quat = None - self.yaw_offset = 0 - - # obstacle distance toolkit - self.scan_distance_to_obstacle = 15000 # 15m min distance in front of robot - self.scan_avg_distance_left = 15000 - self.scan_avg_distance_right = 15000 - self.scan_history = [] - self.median_scan = [] - self.scan_min_window = 10 # we have to see at least 10 points nearer than threshold - self.scan_nr_kept = 3 # we have to see at least 10 points nearer than threshold - - self.avoidance_start = None - self.avoidance_turn = None - self.steering_angle = 0.0 - - self.unsafe_location = None - - self.default_effort_level = 1000 # default is max speed - - self.last_reset_model = None # last time we reset the model (straighten up, drop from 1m) - - self.true_pose = False - self.tf = { - 'vslam': { - 'trans_matrix': None, - 'latest_xyz': None, - 'latest_quat': None, - 'timestamp': None - }, - 'odo': { - 'trans_matrix': None, - 'latest_xyz': (0,0,0), - 'latest_quat': None, - 'timestamp': None - } - } # transformations from position sources (odometry, vslam) to robot coordinates - - self.use_gimbal = True # try to keep the camera on level as we go over obstacles - self.yaw_history = [] - self.pitch_history = [] - self.roll_history = [] - - self.brakes_on = False - self.camera_change_triggered_time = None - self.camera_angle = 0.0 - - self.joint_name = None - - self.score = 0 - self.current_driver = None - - self.inException = False - self.in_driving_recovery = False - - self.last_status_timestamp = None - - self.virtual_bumper = None - - def register(self, callback): - self.monitors.append(callback) - return callback - - def unregister(self, callback): - assert callback in self.monitors - self.monitors.remove(callback) - - def send_speed_cmd(self, speed, angular_speed): - self.bus.publish('desired_speed', [round(speed*1000), round(math.degrees(angular_speed)*100)]) - - def set_cam_angle(self, angle): - angle = min(math.pi / 4.0, max(-math.pi / 8.0, angle)) - self.send_request('set_cam_angle %f\n' % angle) - self.camera_angle = angle - print (self.sim_time, "app: Camera angle set to: %f" % angle) - self.camera_change_triggered_time = self.sim_time - - def set_brakes(self, on): - assert type(on) is bool, on - self.brakes_on = on - self.send_request('set_brakes %s\n' % ('on' if on else 'off')) - print (self.sim_time, self.robot_name, "app: Brakes set to: %s" % on) - - def set_light_intensity(self, intensity): - self.send_request('set_light_intensity %s\n' % intensity) - print (self.sim_time, self.robot_name, "app: Light intensity set to: %s" % intensity) - - def on_driving_recovery(self, data): - self.in_driving_recovery = data - print (self.sim_time, self.robot_name, "Driving recovery changed to: %r" % data) - - def register_origin(self, message): - print (self.sim_time, self.robot_name, "controller: origin received: %s" % (message)) - if message.split()[0] == 'origin': - origin = [float(x) for x in message.split()[1:]] - initial_xyz = origin[:3] - initial_quat = origin[3:] - initial_rpy = euler_zyx(initial_quat) # note: this is not in roll, pitch, yaw order - - self.xyz = initial_xyz - self.xyz_quat = initial_quat - self.yaw_offset = self.yaw - initial_rpy[0] - - for k, obj in self.tf.items(): - # note: if VSLAM is not tracking at time of register_origin call, the latest reported position will be inaccurate and VSLAM won't work - if obj['latest_quat'] is not None: - latest_rpy = euler_zyx(obj['latest_quat']) # will be rearranged after offset calculation - rpy_offset = [a-b for a,b in zip(initial_rpy, latest_rpy)] - rpy_offset.reverse() - print(self.sim_time, self.robot_name, "%s RPY offset: %s" % (k, str(rpy_offset))) - rot_matrix = np.asmatrix(eulerAnglesToRotationMatrix(rpy_offset)) - - xyz_offset = translationToMatrix(obj['latest_xyz']) - orig_xyz_offset = translationToMatrix(initial_xyz) - - obj['trans_matrix'] = np.dot(orig_xyz_offset, np.dot(rot_matrix, xyz_offset.I)) - self.true_pose = True - - - - def calculate_best_pose(self): - # if VSLAM active, report its position either in internal or global coordinates depending on whether origin was established yet - # otherwise use ODO position (internal or global) - - if self.tf['vslam']['timestamp'] is not None and self.sim_time - self.tf['vslam']['timestamp'] < timedelta(milliseconds=300): - if self.tf['vslam']['trans_matrix'] is not None: - v = np.asmatrix(np.array([self.tf['vslam']['latest_xyz'][0], self.tf['vslam']['latest_xyz'][1], self.tf['vslam']['latest_xyz'][2], 1])) - m = np.dot(self.tf['vslam']['trans_matrix'], v.T) - self.xyz = [m[0,0], m[1,0], m[2,0]] - - # use VSLAM pose to update ODO pose - v = np.asmatrix(np.asarray([self.xyz[0], self.xyz[1], self.xyz[2], 1])) - odo = np.dot(self.tf['odo']['trans_matrix'].I, v.T) - self.tf['odo']['latest_xyz'] = [odo[0,0], odo[1,0], odo[2,0]] - - else: - self.xyz = self.tf['odo']['latest_xyz'] - - else: - if self.tf['odo']['trans_matrix'] is not None: - v = np.asmatrix(np.array([self.tf['odo']['latest_xyz'][0], self.tf['odo']['latest_xyz'][1], self.tf['odo']['latest_xyz'][2], 1])) - m = np.dot(self.tf['odo']['trans_matrix'], v.T) - self.xyz = [m[0,0], m[1,0], m[2,0]] - else: - self.xyz = self.tf['odo']['latest_xyz'] - - - self.publish("pose2d", [round(1000*self.xyz[0]), round(1000*self.xyz[1]), round(100*math.degrees(self.yaw))]) - - if self.virtual_bumper is not None: - self.virtual_bumper.update_pose(self.sim_time, (self.xyz[0], self.xyz[1], self.yaw)) - if not self.inException and self.virtual_bumper.collision(): - self.bus.publish('driving_recovery', True) - raise VirtualBumperException() - - def on_desired_speeds(self, data): - linear_speed, angular_speed, steering_angle = data - self.steering_angle = steering_angle - if self.virtual_bumper is not None: - self.virtual_bumper.update_desired_speed(linear_speed, angular_speed) - - def on_vslam_enabled(self, data): - pass - - def on_vslam_pose(self, data): - if self.sim_time is None or self.last_position is None or self.yaw is None: - return - if math.isnan(data[0][0]): # VSLAM not tracking - return - - self.tf['vslam']['latest_xyz'] = data[0] - self.tf['vslam']['latest_quat'] = data[1] - self.tf['vslam']['timestamp'] = self.sim_time - - #print("Internal VSLAM: " + str(self.tf['vslam']['latest_xyz']) + str(self.tf['vslam']['latest_quat'])) - #print("VSLAM XYZ:" + str(self.tf['vslam']['latest_xyz'])) - #print("VSLAM RPY:" + str(euler_zyx(data[1]))) - - self.calculate_best_pose() - - def on_odo_pose(self, data): - x, y, heading = data - pose = (x / 1000.0, y / 1000.0, math.radians(heading / 100.0)) # TODO: use IMU instead of wheels - if self.last_position is not None: - dist = distance(pose, self.last_position) - direction = ((pose[0] - self.last_position[0]) * math.cos(self.last_position[2]) + - (pose[1] - self.last_position[1]) * math.sin(self.last_position[2])) - if direction < 0: - dist = -dist - else: - dist = 0.0 - self.last_position = pose - - x, y, z = self.tf['odo']['latest_xyz'] - x += math.cos(self.pitch) * math.cos(pose[2]) * dist - y += math.cos(self.pitch) * math.sin(pose[2]) * dist - z += math.sin(self.pitch) * dist - self.tf['odo']['latest_xyz'] = x, y, z - self.tf['odo']['latest_quat'] = euler_to_quaternion(self.yaw, self.pitch, self.roll) - self.tf['odo']['timestamp'] = self.sim_time - - self.calculate_best_pose() - - - def on_driving_control(self, data): - # someone else took over driving - self.current_driver = data - - def on_score(self, data): - self.score = data[0] - - def on_scan(self, data): - # data is 180 samples, first and last 40 are 0 - # NASA Lidar 150degrees wide, 100 samples - # robot is ~2.21m wide (~1.2m x 2 with wiggle room) - # 40 samples represents 60 degrees (1.0472rad) - - IDEAL_SAMPLE_SIZE = 30 # number of samples on each side of midpoint - collision_view = [x if x > 10 else 15000 for x in data] - self.scan_history.append(collision_view) - if len(self.scan_history) > self.scan_nr_kept: - self.scan_history.pop(0) - else: - return - - median_scan = [] - for j in range(len(collision_view)): - median_scan.append(median([self.scan_history[i][j] for i in range(len(self.scan_history))])) - self.median_scan = median_scan - - # measure distance only in 66 degree angle (about the width of the robot 1.5m) - # however, don't look just ahead (camera angle) or direction of wheels (steering angle) - # look further in the direction wheels are turned - midpoint = int(max(50, min(130, 40 + 50 + 2 * self.steering_angle / LIDAR_BEAM_SPACING))) - if midpoint < 40 + IDEAL_SAMPLE_SIZE: - sample_size = midpoint - 40 - elif midpoint > 140 - IDEAL_SAMPLE_SIZE: - sample_size = 140 - midpoint - else: - sample_size = IDEAL_SAMPLE_SIZE - - distances = median_scan[midpoint:midpoint + sample_size] - mean = np.mean(distances) - sd = np.std(distances) - d_clean = [x for x in distances if mean - 2 * sd < x < mean + 2 * sd] - #self.scan_avg_distance_left = sum(d_clean) / len(d_clean) if len(d_clean) > 1 else 15000 - self.scan_avg_distance_left = min(d_clean) if len(d_clean) > 1 else 15000 - - distances = median_scan[midpoint - sample_size:midpoint] - mean = np.mean(distances) - sd = np.std(distances) - d_clean = [x for x in distances if mean - 2 * sd < x < mean + 2 * sd] - #self.scan_avg_distance_right = sum(d_clean) / len(d_clean) if len(d_clean) > 1 else 15000 - self.scan_avg_distance_right = min(d_clean) if len(d_clean) > 1 else 15000 - - #self.scan_avg_distance_left = sum(median_scan[midpoint:midpoint + sample_size]) / sample_size - #self.scan_avg_distance_right = sum(median_scan[midpoint - sample_size:midpoint]) / sample_size - before_robot = median_scan[midpoint - sample_size:midpoint + sample_size] - before_robot.sort(); - self.scan_distance_to_obstacle = median(before_robot[:self.scan_min_window]) - - # unless we are already inside unsafe area, augment nearest obstacles with virtual fence values - if self.unsafe_location is not None and not self.unsafe_location.contains(Point(self.xyz[0], self.xyz[1])): - # calculate position in front of and to the left and right of the robot in the direction of its heading - v = np.asmatrix(np.asarray([self.xyz[0], self.xyz[1], 1])) - c, s = np.cos(self.steering_angle + self.yaw), np.sin(self.steering_angle + self.yaw) - R = np.asmatrix(np.array(((c, -s, 0), (s, c, 0), (0, 0, 1)))) - - T = np.asmatrix(np.array(((1, 0, VIRTUAL_FENCE_LOOK_AHEAD_DISTANCE),(0, 1, 0),(0, 0, 1)))) - pos = np.dot(R, np.dot(T, np.dot(R.I, v.T))) - ls = LineString([(self.xyz[0], self.xyz[1]), (float(pos[0]), float(pos[1]))]) - intr = self.unsafe_location.intersection(ls) - if not intr.is_empty: - d = int(1000*(ls.length - intr.length)) - if self.debug: - print(self.sim_time, self.robot_name, "Virtual fence: decreasing front distance to %d" % d) - self.scan_distance_to_obstacle = min(self.scan_distance_to_obstacle, d) - - left_angle = IDEAL_SAMPLE_SIZE * LIDAR_BEAM_SPACING - x,y = pol2cart(VIRTUAL_FENCE_LOOK_AHEAD_DISTANCE, left_angle) - Tl = np.asmatrix(np.array(((1, 0, x),(0, 1, y),(0, 0, 1)))) - posL = np.dot(R, np.dot(Tl, np.dot(R.I, v.T))) - ls = LineString([(self.xyz[0], self.xyz[1]), (float(posL[0]), float(posL[1]))]) - intr = self.unsafe_location.intersection(ls) - if not intr.is_empty: - d = int(1000*(ls.length - intr.length)) - if self.debug: - print(self.sim_time, self.robot_name, "Virtual fence: decreasing left distance to %d" % d) - self.scan_avg_distance_left = min(self.scan_avg_distance_left, d) - - right_angle = -IDEAL_SAMPLE_SIZE * LIDAR_BEAM_SPACING - x,y = pol2cart(VIRTUAL_FENCE_LOOK_AHEAD_DISTANCE, right_angle) - Tr = np.asmatrix(np.array(((1, 0, x),(0, 1, y),(0, 0, 1)))) - posR = np.dot(R, np.dot(Tr, np.dot(R.I, v.T))) - ls = LineString([(self.xyz[0], self.xyz[1]), (float(posR[0]), float(posR[1]))]) - intr = self.unsafe_location.intersection(ls) - if not intr.is_empty: - d = int(1000*(ls.length - intr.length)) - if self.debug: - print(self.sim_time, self.robot_name, "Virtual fence: decreasing right distance to %d" % d) - self.scan_avg_distance_right = min(self.scan_avg_distance_right, d) - - - def on_joint_position(self, data): - assert self.joint_name is not None - self.sensor_joint_position = data[self.joint_name.index(b'sensor_joint')] - - def on_rot(self, data): - # use of on_rot is deprecated, will be replaced by on_orientation - # also, this filtering does not work when an outlier is presented while angles near singularity - # e.g., values 0, 1, 359, 358, 120 will return 120 - temp_yaw, temp_pitch, temp_roll = [normalizeAnglePIPI(math.radians(x/100)) for x in data] - - self.yaw_history.append(temp_yaw) - self.pitch_history.append(temp_pitch) - self.roll_history.append(temp_roll) - if len(self.yaw_history) > 5: - self.yaw_history.pop(0) - self.pitch_history.pop(0) - self.roll_history.pop(0) - - self.yaw = normalizeAnglePIPI(median(self.yaw_history) - self.yaw_offset) - self.pitch = median(self.pitch_history) - self.roll = median(self.roll_history) - - if self.use_gimbal: - # maintain camera level - cam_angle = min(math.pi / 4.0, max(-math.pi / 8.0, self.camera_angle + self.pitch)) - self.publish('cmd', b'set_cam_angle %f' % cam_angle) - - if self.virtual_bumper is not None and not self.inException and (abs(self.pitch) > 0.9 or abs(self.roll) > math.pi/4): - # TODO pitch can also go the other way if we back into an obstacle - # TODO: robot can also roll if it runs on a side of a rock while already on a slope - self.bus.publish('driving_recovery', True) - print (self.sim_time, self.robot_name, "app: Excess pitch or roll, going back") - raise VirtualBumperException() - - if abs(self.roll) > math.pi/2 and (self.last_reset_model is None or self.sim_time - self.last_reset_model > timedelta(seconds=15)): - # if roll is more than 90deg, robot is upside down - self.last_reset_model = self.sim_time - # self.send_request('reset_model') - - def get_extra_status(self): - return "" - - def update(self): - - # print status periodically - location - if self.sim_time is not None: - if self.last_status_timestamp is None: - self.last_status_timestamp = self.sim_time - elif self.sim_time - self.last_status_timestamp > timedelta(seconds=8) and self.xyz is not None: - self.last_status_timestamp = self.sim_time - x, y, z = self.xyz - s = ps("---------------------------------------------------------") - s += ps(self.sim_time, "%s:" % self.robot_name) - s += ps("RPY: [%f %f %f]; Driver: %s; Score: %d" % (self.roll, self.pitch, self.yaw, self.current_driver, self.score)) - s += ps("Loc[best]: [%f %f %f]" % (x, y, z)) - for k, obj in self.tf.items(): - if obj['trans_matrix'] is not None and self.sim_time - obj['timestamp'] < timedelta(milliseconds=300): - x, y, z = obj['latest_xyz'] - v = np.asmatrix(np.array([x, y, z, 1])) - m = np.dot(obj['trans_matrix'], v.T) - x,y,z = [m[0,0], m[1,0], m[2,0]] - s += ps("Loc[%s]: [%f %f %f]" % (k, x, y, z)) - s += self.get_extra_status() - s += ("\n---------------------------------------------------------") - print(s) - - channel = super().update() - return channel - - def get_angle_diff(self, destination, direction=1): - angle_diff = normalizeAnglePIPI(math.atan2(destination[1] - self.xyz[1], destination[0] - self.xyz[0]) - self.yaw) - if direction < 0: - angle_diff = - math.pi + angle_diff - - return normalizeAnglePIPI(angle_diff) - - def get_avoidance_turn(self): - turn = None - - # if avoidance expired, reset turn - if self.avoidance_start is not None and self.sim_time - self.avoidance_start >= timedelta(milliseconds=AVOIDANCE_DURATION): - self.avoidance_turn = None - - # finish existing avoidance - if self.avoidance_start is not None: - if self.sim_time - self.avoidance_start < timedelta(milliseconds=AVOIDANCE_DURATION): - if self.sim_time - self.avoidance_start < timedelta(milliseconds=AVOIDANCE_TURN_DURATION): - if self.debug: - print(self.sim_time, self.robot_name, "Still turning") - turn = self.avoidance_turn # keep turning 0.5s after obstacle no longer visible - - elif self.sim_time - self.avoidance_start < timedelta(milliseconds=AVOIDANCE_DURATION): - if self.debug: - print(self.sim_time, self.robot_name, "Going straight") - turn = GO_STRAIGHT # go straight additional 1s after no longer turning - else: - self.avoidance_turn = None - - if self.scan_distance_to_obstacle < 4000: - # optionally do not avoid obstacles when real close to destination - if destination is a volatile, you know the obstacle is just a bump - self.avoidance_start = self.sim_time - if self.scan_avg_distance_left < 0.8 * self.scan_avg_distance_right: - self.avoidance_turn = -AVOID_RADIUS # steer to the right - elif self.scan_avg_distance_left * 0.8 > self.scan_avg_distance_right: - self.avoidance_turn = AVOID_RADIUS - else: - if self.avoidance_turn is None: - self.avoidance_turn = AVOID_RADIUS if self.rand.getrandbits(1) == 0 else -AVOID_RADIUS - turn = self.avoidance_turn - if (self.debug): - print(self.sim_time, self.robot_name, "Seeing object, turning: %d, distance %d" % (turn, self.scan_distance_to_obstacle)) - - return turn - - - def go_to_location(self, pos, desired_speed, offset=0.0, full_turn=False, timeout=None, with_stop=True, tolerance=1.0, avoid_obstacles_close_to_destination=False): - # speed: + forward, - backward - # offset: stop before (-) or past (+) the actual destination (e.g., to keep the destination in front of the robot) - print(self.sim_time, self.robot_name, "go_to_location [%.1f,%.1f] + offset %.1f (speed: %.1f)" % (pos[0], pos[1], offset, math.copysign(self.max_speed, desired_speed))) - - dist = distance(pos, self.xyz)+offset - if timeout is None: - timeout = timedelta(seconds=(10 if full_turn else 0) + 2*dist / self.max_speed) # 30m should take at most 60 seconds, unless turning heavily, then add the amount of time it takes to turn around - - start_time = self.sim_time - - # while we are further than tolerance or while the distance from target is decreasing but still following the right direction (angle diff < 90deg) - angle_diff = self.get_angle_diff(pos,desired_speed) - last_dist = distance(pos, self.xyz) + offset - while (distance(pos, self.xyz)+offset > tolerance or distance(pos, self.xyz)+offset < last_dist) and (full_turn or abs(angle_diff) < math.pi/2): - last_dist = distance(pos, self.xyz)+offset - if last_dist < 0: - break - angle_diff = self.get_angle_diff(pos,desired_speed) - speed = desired_speed if last_dist > 4 or not with_stop else 0.6 * desired_speed - - if self.debug: - print(self.sim_time, self.robot_name, "Distance ahead: %.1f, Left avg distance: %.1f, Right avg distance: %.1f" % (self.scan_distance_to_obstacle, self.scan_avg_distance_left, self.scan_avg_distance_right)) - - turn = None - if ( - speed > 0 and - (avoid_obstacles_close_to_destination or last_dist > 5) - ): - turn = self.get_avoidance_turn() - - if turn is None: - if angle_diff > 0.1 and last_dist > 1: - turn = TURN_RADIUS * math.copysign(1, speed) - elif angle_diff < -0.1 and last_dist > 1: - turn = -TURN_RADIUS * math.copysign(1, speed) - else: - turn = GO_STRAIGHT - self.publish("desired_movement", [turn, 0, speed]) - - self.wait(timedelta(milliseconds=50)) - angle_diff = self.get_angle_diff(pos,speed) # update angle again after wait just before the while loop - - if timeout is not None and self.sim_time - start_time > timeout: - print(self.sim_time, self.robot_name, "go_to_location timeout ended at [%.1f,%.1f]" % (self.xyz[0], self.xyz[1])) - break - if with_stop: - self.send_speed_cmd(0.0, 0.0) - print(self.sim_time, self.robot_name, "go_to_location [%.1f,%.1f] ended at [%.1f,%.1f], yaw=%.2f, angle_diff=%f" % (pos[0], pos[1], self.xyz[0], self.xyz[1], self.yaw, angle_diff)) - - def move_sideways(self, how_far, view_direction=None, timeout=None): # how_far: left is positive, right is negative - print(self.sim_time, self.robot_name, "move_sideways %.1f (speed: %.1f)" % (how_far, self.max_speed), self.last_position) - if timeout is None: - timeout = timedelta(seconds=4 + 2*abs(how_far) / self.max_speed) # add 4 sec for wheel setup - if view_direction is not None: - angle_diff = normalizeAnglePIPI(view_direction - self.yaw) - if abs(angle_diff) > math.radians(10): # only turn if difference more than 10deg - self.turn(angle_diff, timeout=timedelta(seconds=10)) - - start_pose = self.xyz - start_time = self.sim_time - while distance(start_pose, self.xyz) < abs(how_far): - self.publish("desired_movement", [GO_STRAIGHT, -9000, -math.copysign(self.default_effort_level, how_far)]) - self.wait(timedelta(milliseconds=100)) - if timeout is not None and self.sim_time - start_time > timeout: - print(self.sim_time, self.robot_name, "go_sideways - timeout at %.1fm" % distance(start_pose, self.xyz)) - break - self.send_speed_cmd(0.0, 0.0) - print(self.sim_time, self.robot_name, "move sideways ended at [%.1f,%.1f]" % (self.xyz[0], self.xyz[1])) - - - def go_straight(self, how_far, with_stop=True, timeout=None): - print(self.sim_time, self.robot_name, "go_straight %.1f (speed: %.1f)" % (how_far, self.max_speed), self.last_position) - if timeout is None: - timeout = timedelta(seconds=10 + 2*abs(how_far) / self.max_speed) - - start_pose = self.last_position - if how_far >= 0: - self.send_speed_cmd(self.max_speed, 0.0) - else: - self.send_speed_cmd(-self.max_speed, 0.0) - start_time = self.sim_time - slowdown_happened = False - while distance(start_pose, self.last_position) < abs(how_far): - if with_stop and not slowdown_happened and abs(distance(start_pose, self.last_position) - abs(how_far)) < 2: - self.send_speed_cmd(math.copysign(self.max_speed / 2.0, how_far), 0.0) - slowdown_happened = True - - self.update() - if timeout is not None and self.sim_time - start_time > timeout: - print(self.sim_time, self.robot_name, "go_straight - timeout at %.1fm" % distance(start_pose, self.last_position)) - break - self.send_speed_cmd(0.0, 0.0) - - def turn(self, angle, with_stop=True, speed=0.0, ang_speed=None, timeout=None): - # positive turn - counterclockwise - print(self.sim_time, self.robot_name, "turn %.1f deg from %.1f deg" % (math.degrees(angle), math.degrees(self.yaw))) - self.send_speed_cmd(speed, math.copysign(ang_speed if ang_speed is not None else self.max_angular_speed, angle)) - - start_time = self.sim_time - # problem with accumulated angle - - sum_angle = 0.0 - prev_angle = self.yaw - slowdown_happened = False - while abs(sum_angle) < abs(angle): - if with_stop and not slowdown_happened and abs(sum_angle) > abs(angle) - 0.35: # slow rotation down for the last 20deg to improve accuracy - self.send_speed_cmd(speed, math.copysign(self.max_angular_speed / 2.0, angle)) - slowdown_happened = True - - self.update() - sum_angle += normalizeAnglePIPI(self.yaw - prev_angle) - prev_angle = self.yaw - if timeout is not None and self.sim_time - start_time > timeout: - print(self.sim_time, self.robot_name, "turn - timeout at %.1fdeg" % math.degrees(sum_angle)) - break - if with_stop: - self.send_speed_cmd(0.0, 0.0) - - def wait(self, dt): # TODO refactor to some common class - while self.sim_time is None: - self.update() - start_sim_time = self.sim_time - while self.sim_time - start_sim_time < dt: - self.update() - - def lidar_drive_around(self, direction=None): - # direction: only sign counts, positive will step around to the left, negative to the right - print(self.sim_time, self.robot_name, "lidar_drive_around (speed: %.1f)" % (self.max_speed)) - timeout = timedelta(seconds=10) # add 4 sec for wheel setup - - # if substantially more close points to the left, go right and vice versa; if similar, go in one direction at random (avoids getting stuck) - if direction is None: - if self.scan_avg_distance_left < 0.8 * self.scan_avg_distance_right: - direction = -1 - elif self.scan_avg_distance_left * 0.8 > self.scan_avg_distance_right: - direction = 1 - else: - direction = 1 if self.rand.getrandbits(1) == 0 else -1 - - start_pose = self.xyz - start_time = self.sim_time - while self.scan_distance_to_obstacle < 4000: # 4m - self.publish("desired_movement", [GO_STRAIGHT, -math.copysign(5500, direction), -self.default_effort_level]) # 1000m radius is almost straight - self.wait(timedelta(milliseconds=100)) - if timeout is not None and self.sim_time - start_time > timeout: - print(self.sim_time, self.robot_name, "lidar_drive_around - timeout at %.1fm" % distance(start_pose, self.xyz)) - break - - def drive_around_rock(self, how_far, view_direction=None, timeout=None): - # go around a rock with 'how_far' clearance, if how_far positive, it goes around to the left, negative means right - # will keeping to look forward (i.e, movement is sideways) - # view_direction - if present, first turn in place to point in that direction - # TODO: use lidar to go around as much as needed - print(self.sim_time, self.robot_name, "go_around_a_rock %.1f (speed: %.1f)" % (how_far, self.max_speed)) - if timeout is None: - timeout = timedelta(seconds=4 + 2*abs(how_far) / self.max_speed) # add 4 sec for wheel setup - if view_direction is not None: - self.turn(normalizeAnglePIPI(view_direction - self.yaw), timeout=timedelta(seconds=10)) - - start_pose = self.xyz - start_time = self.sim_time - while distance(start_pose, self.xyz) < abs(how_far): - self.publish("desired_movement", [GO_STRAIGHT, 7500, -math.copysign(self.default_effort_level, how_far)]) # 1000m radius is almost straight - self.wait(timedelta(milliseconds=100)) - if timeout is not None and self.sim_time - start_time > timeout: - print(self.sim_time, self.robot_name, "go_around_a_rock - timeout at %.1fm" % distance(start_pose, self.xyz)) - break - - self.send_speed_cmd(0.0, 0.0) # TEST - return # TEST TODO: this will end rock bypass routine right after side-step - - self.go_straight(math.copysign(abs(how_far) + 5, how_far)) # TODO: rock size estimate - # TODO: maybe going back not needed, hand over to main routine, we presumably already cleared the obstacle - start_pose = self.xyz - start_time = self.sim_time - while distance(start_pose, self.xyz) < abs(how_far): - self.publish("desired_movement", [GO_STRAIGHT, -7500, -math.copysign(self.default_effort_level, how_far)]) # 1000m radius is almost straight - self.wait(timedelta(milliseconds=100)) - if timeout is not None and self.sim_time - start_time > timeout: - print(self.sim_time, self.robot_name, "go_around_a_rock - timeout at %.1fm" % distance(start_pose, self.xyz)) - break - self.send_speed_cmd(0.0, 0.0) - print(self.sim_time, self.robot_name, "go_around_a_rock ended at [%.1f,%.1f]" % (self.xyz[0], self.xyz[1])) - - - def try_step_around(self): - self.turn(math.radians(90), timeout=timedelta(seconds=10)) - - # recovered enough at this point to switch to another driver (in case you see cubesat while doing the 3m drive or the final turn) - self.bus.publish('driving_recovery', False) - - self.go_straight(5.0) - self.turn(math.radians(-90), timeout=timedelta(seconds=10)) - - def wait_for_init(self): - print(self.robot_name, 'Wait for definition of last_position and yaw') - while self.sim_time is None or self.last_position is None or self.yaw is None: - try: - self.update() - except BusShutdownException: - raise - except: - pass - print(self.robot_name, 'done at', self.sim_time) - -# vim: expandtab sw=4 ts=4 diff --git a/moon/controller_excavator_round2.py b/moon/controller_excavator_round2.py deleted file mode 100644 index 5c4cc8f92..000000000 --- a/moon/controller_excavator_round2.py +++ /dev/null @@ -1,1013 +0,0 @@ -""" - Space Robotics Challenge 2 -""" -import numpy as np -import math -from datetime import timedelta -import traceback -import sys -from functools import cmp_to_key -import cv2 - -from shapely.geometry import Point, LineString, MultiPolygon, Polygon -from statistics import median - -from osgar.bus import BusShutdownException - -from moon.controller import eulerAnglesToRotationMatrix, translationToMatrix, GO_STRAIGHT, TURN_RADIUS, calc_tangents, pol2cart, ps, distance, SpaceRoboticsChallenge, MoonException, VirtualBumperException, LidarCollisionException, LidarCollisionMonitor, VSLAMEnabledException, VSLAMDisabledException, VSLAMLostException, VSLAMFoundException -from osgar.lib.virtual_bumper import VirtualBumper -from osgar.lib.quaternion import euler_zyx - -from osgar.lib.mathex import normalizeAnglePIPI - -DIG_GOOD_LOCATION_MASS = 10 -GROUP_ANGLES_MAX_DIFF = 0.3 -ARRIVAL_OFFSET = -1.5 -TYPICAL_VOLATILE_DISTANCE = 2 -DISTAL_THRESHOLD = 0.145 # TODO: movement may be too fast to detect when the scooping first happened -PREFERRED_POLYGON = Polygon([(-32,9),(-25,20),(-16,26),(-6,26),(3.5,22),(14.5,25.5),(23.5,20),(30,9),(35,1),(30,-13.5),(21,-24),(11,-30),(5,-36),(-18,-32),(-19,-17),(-26,-4)]) - -SAFE_POLYGON = PREFERRED_POLYGON.union(MultiPolygon([ - Polygon([(-30,9),(-43,15),(-41,25),(-28,38),(-15,44.5),(-13,50.5),(-33,51),(-38,36),(-53,26),(-61,3),(-26,-1)]), - Polygon([(-45,-39),(-58,-40),(-59,-50),(43,-50),(43,-31),(60,-27),(65,30),(51,15),(46,-18), (29,-12.5), (27, -15), (43,-22), (27,-37),(4,-31),(-15,-30)]) - ])) - -APPROACH_DISTANCE = 2.5 - -class WaitRequestedException(MoonException): - pass -class ResumeRequestedException(MoonException): - pass -class NewVolatileTargetException(MoonException): - pass - -class SpaceRoboticsChallengeExcavatorRound2(SpaceRoboticsChallenge): - def __init__(self, config, bus): - super().__init__(config, bus) - bus.register("bucket_dig", "bucket_drop") - self.volatile_dug_up = None - self.mount_angle = None - self.distal_angle = None - self.vol_list = None - self.vslam_reset_at = None - self.use_gimbal = False - self.robot_name = "excavator_1" - self.hauler_ready = False - self.vslam_is_enabled = False - self.hauler_orig_pose = None - self.full_360_objects = {} - self.full_360_distances = [] - self.first_distal_angle = None - self.hauler_pose_requested = False - self.auto_light_adjustment = False - self.light_intensity = 0.0 - self.vslam_fail_start = None - self.vslam_valid = False - self.visual_navigation_enabled = False - self.lidar_distance_enabled = False - self.current_volatile = None - self.going_to_volatile = False - self.unsafe_location = Polygon([(-100, -100), (-100, 100), (100, 100), (100, -100)]).difference(SAFE_POLYGON) - - def on_osgar_broadcast(self, data): - message_target = data.split(" ")[0] - if message_target == self.robot_name: - print(self.sim_time, self.robot_name, "Received external_command: %s" % str(data)) - command = data.split(" ")[1] - if command == "arrived": - self.hauler_yaw = float(data.split(" ")[2]) - self.hauler_ready = True - if command == "wait": - raise WaitRequestedException() - if command == "resume": - raise ResumeRequestedException() - if command == "hauler_true_pose": - # assumes hauler finished approach as is behind excavator in self.yaw direction - self.hauler_pose_requested = True - print(self.sim_time, self.robot_name, "Origin of hauler received: ", str(data.split(" ")[2:])) - if data.split()[2] == 'origin': - origin = [float(x) for x in data.split()[3:]] - initial_xyz = origin[:3] - initial_quat = origin[3:] - initial_rpy = euler_zyx(initial_quat) # note: this is not in roll, pitch, yaw order - - v = np.asmatrix(np.asarray([initial_xyz[0], initial_xyz[1], 1])) - c, s = np.cos(initial_rpy[0]), np.sin(initial_rpy[0]) - Rr = np.asmatrix(np.array(((c, -s, 0), (s, c, 0), (0, 0, 1)))) - T = np.asmatrix(np.array(((1, 0, APPROACH_DISTANCE),(0, 1, 0),(0, 0, 1)))) # looking for location in front of origin - ex_pos = np.dot(Rr, np.dot(T, np.dot(Rr.I, v.T))) - print(self.sim_time, self.robot_name, "Adjusting XY based on hauler origin: from [%.1f,%.1f] to [%.1f,%.1f]" % (self.xyz[0], self.xyz[1], float(ex_pos[0]), float(ex_pos[1]))) - - if self.tf['vslam']['trans_matrix'] is not None: - m = translationToMatrix([float(ex_pos[0])-self.xyz[0], float(ex_pos[1])-self.xyz[1], 0.0]) - self.tf['vslam']['trans_matrix'] = np.dot(m, self.tf['vslam']['trans_matrix']) - else: - ex_initial_xyz = [float(ex_pos[0]), float(ex_pos[1]), origin[2]] - initial_rpy[0] = self.yaw # keep existing yaw - - self.xyz = ex_initial_xyz - self.xyz_quat = initial_quat - - for k, obj in self.tf.items(): - # note: if VSLAM is not tracking at time of register_origin call, the latest reported position will be inaccurate and VSLAM won't work - if obj['latest_quat'] is not None: - latest_rpy = euler_zyx(obj['latest_quat']) # will be rearranged after offset calculation - rpy_offset = [a-b for a,b in zip(initial_rpy, latest_rpy)] - rpy_offset.reverse() - print(self.sim_time, self.robot_name, "%s RPY offset: %s" % (k, str(rpy_offset))) - rot_matrix = np.asmatrix(eulerAnglesToRotationMatrix(rpy_offset)) # TODO: save the original rot matrix - - xyz_offset = translationToMatrix(obj['latest_xyz']) - orig_xyz_offset = translationToMatrix(ex_initial_xyz) - - obj['trans_matrix'] = np.dot(orig_xyz_offset, np.dot(rot_matrix, xyz_offset.I)) - - - else: - print(self.sim_time, self.robot_name, "Invalid origin format") - - if command == "pose": - self.hauler_orig_pose = [float(data.split(" ")[2]),float(data.split(" ")[3])] - - def on_left_image(self, data): - if not self.auto_light_adjustment: - return - limg = cv2.imdecode(np.frombuffer(data, dtype=np.uint8), cv2.IMREAD_COLOR) - CAMERA_HEIGHT,CAMERA_WIDTH, _ = limg.shape - hsv = cv2.cvtColor(limg, cv2.COLOR_BGR2HSV) - mask = np.zeros((CAMERA_HEIGHT,CAMERA_WIDTH), np.uint8) - circle_mask = cv2.circle(mask,(CAMERA_HEIGHT//2,CAMERA_WIDTH//2),200,(255,255,255),thickness=-1) - hist = cv2.calcHist([limg],[2],circle_mask,[256],[0,256]) - topthird = hist[170:] - brightness = int(sum(topthird) / len(topthird)) - if brightness < 300: - self.light_intensity = min(1.0, self.light_intensity + 0.1) - self.send_request('set_light_intensity %s' % str(self.light_intensity)) - elif brightness > 500: - self.light_intensity = max(0.0, self.light_intensity - 0.1) - self.send_request('set_light_intensity %s' % str(self.light_intensity)) - - def on_vslam_enabled(self, data): - super().on_vslam_enabled(data) - if self.vslam_is_enabled != data: - self.vslam_is_enabled = data - - if not self.true_pose or self.sim_time is None or self.last_position is None or self.yaw is None: - return - - if self.vslam_is_enabled: - raise VSLAMEnabledException() - else: - raise VSLAMDisabledException() - - def get_extra_status(self): - if self.volatile_dug_up[1] == 100: - return "Bucket empty" - else: - return ps("Bucket content: Type: %s idx: %d mass: %f" % (self.volatile_dug_up[0], self.volatile_dug_up[1], self.volatile_dug_up[2])) - - def on_bucket_info(self, bucket_status): - #[0] .. type ('ice'); [1] .. index (100=nothing); [2] .. mass - self.volatile_dug_up = bucket_status - if self.first_distal_angle is None and bucket_status[1] != 100: - print (self.sim_time, self.robot_name, "Volatile in bucket at distal: %.2f" % self.distal_angle) - self.first_distal_angle = self.distal_angle - - def on_joint_position(self, data): - super().on_joint_position(data) - self.mount_angle = data[self.joint_name.index(b'mount_joint')] - self.distal_angle = data[self.joint_name.index(b'distalarm_joint')] - - def on_sim_clock(self, data): - super().on_sim_clock(data) - if self.going_to_volatile and data[0] % 5 == 0 and data[1] == 0: - # if pursuing volatile then every two seconds rerun trajectory optimization and if better volatile than planned ahead, re-plan route - v = self.get_next_volatile() - if v != self.current_volatile and distance(self.xyz, self.current_volatile) > 6: # do not change volatile if almost arrived to the current one - print (self.sim_time, self.robot_name, "New volatile target: [%.1f,%.1f]" % (v[0], v[1])) - self.current_volatile = v - raise NewVolatileTargetException() - - - def on_vslam_pose(self, data): - super().on_vslam_pose(data) - - if self.sim_time is None or self.last_position is None or self.yaw is None: - return - - if math.isnan(data[0][0]) and self.tf['vslam']['trans_matrix'] is not None and not self.hauler_pose_requested: # VSLAM not tracking and potential for re-tracking via hauler - self.vslam_valid = False - if self.vslam_fail_start is None: - self.vslam_fail_start = self.sim_time - elif self.sim_time - self.vslam_fail_start > timedelta(milliseconds=300): - self.vslam_fail_start = None - raise VSLAMLostException() - return - - self.vslam_fail_start = None - if not math.isnan(data[0][0]): - self.vslam_valid = True - - if not self.true_pose and self.vslam_reset_at is not None and self.sim_time - self.vslam_reset_at > timedelta(seconds=3) and not math.isnan(data[0][0]) and self.tf['vslam']['trans_matrix'] is None: - # request origin and start tracking in correct coordinates as soon as first mapping lock occurs - # TODO: another pose may arrive while this request is still being processed (not a big deal, just a ROS error message) - self.send_request('request_origin', self.register_origin) - - def get_next_volatile(self): - # first, remove volatiles that we won't attempt - in unsafe areas, too close or in the direction of sun - accessible_volatiles = [] - rover = np.asmatrix(np.asarray([self.xyz[0], self.xyz[1], 1])) - c, s = np.cos(self.yaw), np.sin(self.yaw) - R = np.asmatrix(np.array(((c, -s, 0), (s, c, 0), (0, 0, 1)))) - TL = np.asmatrix(np.array(((1, 0, 0),(0, 1, -8),(0, 0, 1)))) - TR = np.asmatrix(np.array(((1, 0, 0),(0, 1, 8),(0, 0, 1)))) - pL = np.dot(R, np.dot(TL, np.dot(R.I, rover.T))) - pR = np.dot(R, np.dot(TR, np.dot(R.I, rover.T))) - turn_center_L = (float(pL[0]), float(pL[1])) - turn_center_R = (float(pR[0]), float(pR[1])) - - for v in self.vol_list: - - d = distance(self.xyz, v) - if not SAFE_POLYGON.contains(Point(v[0],v[1])): - print (self.sim_time, self.robot_name, "[%.1f,%.1f] not in safe area" % (v[0], v[1])) - continue - accessible_volatiles.append(v) - - # balance distance, angle and length of traversing non-preferred areas - # want to avoid non-preferred areas the most but if expose similar, choose volatile with shorter travel distance - # we are modelling an initial turn along a circle followed by straight drive to the destination - ranked_list = [] - - approx = 16 - arc_len = math.pi/approx - arc_dist = 2 * math.pi * TURN_RADIUS / approx - - # destination point outside turn circle per above test - for v in accessible_volatiles: - path_to_vol = [] - c_angular_difference = self.get_angle_diff(v) - turn_center = turn_center_L if c_angular_difference < 0 else turn_center_R - tangs = calc_tangents(turn_center, TURN_RADIUS, v) - if tangs is not None: - tang1_v = np.asmatrix(np.asarray([tangs[0][0], tangs[0][1], 1])) - tang2_v = np.asmatrix(np.asarray([tangs[1][0], tangs[1][1], 1])) - - i = 1; - while True: - phi = -math.copysign(math.pi/2, c_angular_difference) + self.yaw + math.copysign(i*arc_len, c_angular_difference) - i += 1 - x,y = pol2cart(TURN_RADIUS, phi) - px = turn_center[0] + x - py = turn_center[1] + y - yw = phi - math.pi/2 - path_to_vol.append((px, py)) - - rover_t = np.asmatrix(np.array(((1, 0, px),(0, 1, py),(0, 0, 1)))) - c, s = np.cos(yw), np.sin(yw) - rover_r = np.asmatrix(np.array(((c, -s, 0), (s, c, 0), (0, 0, 1)))) - mat = np.dot(rover_r, rover_t.I) - m1 = np.dot(mat, tang1_v.T) - m2 = np.dot(mat, tang2_v.T) - - if ( - (distance([m1[0,0], m1[1,0]], [0,0]) < arc_dist+0.1 and m1[0,0]>=-0.1) or - (distance([m2[0,0], m2[1,0]], [0,0]) < arc_dist+0.1 and m2[0,0]>=-0.10) - ): - break - - else: - # if we had to go with a volatile within the turning radius, do not build a driving arc, just use straight line - path_to_vol.append((self.xyz[0],self.xyz[1])) - path_to_vol.append((v[0],v[1])) - ls = LineString(path_to_vol).buffer(1) - c_nonpreferred_overlap = ls.difference(PREFERRED_POLYGON).area - - ranked_list.append([v, c_angular_difference, c_nonpreferred_overlap, ls.area]) - - # ranked_list: [volatile, angle, area, distance] - def cmp(a,b): - nonlocal turn_center_L, turn_center_R - #a: [[x,y], angular_diff, non_preferred_overlap, distance_along_driving_path] - a_v, _, a_overlap, a_distance = a - b_v, _, b_overlap, b_distance = b - - - a_cost = 100 if ( - Point(turn_center_L[0],turn_center_L[1]).buffer(TURN_RADIUS+0.1).contains(Point(a_v[0],a_v[1])) or - Point(turn_center_R[0],turn_center_R[1]).buffer(TURN_RADIUS+0.1).contains(Point(a_v[0],a_v[1])) - ) else 0 - b_cost = 100 if ( - Point(turn_center_L[0],turn_center_L[1]).buffer(TURN_RADIUS+0.1).contains(Point(b_v[0],b_v[1])) or - Point(turn_center_R[0],turn_center_R[1]).buffer(TURN_RADIUS+0.1).contains(Point(b_v[0],b_v[1])) - ) else 0 - - a_cost += 200 if -3*math.pi/4 < math.atan2(a_v[1] - self.xyz[1], a_v[0] - self.xyz[0]) < -math.pi/4 and a_distance > 15 else 0 - a_cost += 200 if -3*math.pi/4 < math.atan2(a_v[1] - self.xyz[1], a_v[0] - self.xyz[0]) < -math.pi/4 and a_distance > 15 else 0 - - a_cost += a_distance - b_cost += b_distance - - return a_cost - b_cost if abs(a_overlap - b_overlap) < 10 else a_overlap - b_overlap - - ranked_list = sorted(ranked_list, key=cmp_to_key(cmp)) # if non safe overlap similar, focus on distance - print (self.sim_time, self.robot_name, "List of volatiles in the order of suitability for traveling to: ", ranked_list) - return ranked_list[0][0] - - def on_scan(self, data): - super().on_scan(data) - if self.sim_time is None or self.true_pose: - return - - if self.lidar_distance_enabled: - self.full_360_distances.append({'yaw': self.yaw, 'distance': self.scan_distance_to_obstacle}) - - - def on_artf(self, data): - artifact_type = data[0] - - # can't use this before init and don't need once true pose has been set up - if self.sim_time is None or self.true_pose: - return - - # NOTE: counting starts before turning, ie if hauler is visible in the beginning, it keeps getting hits - if self.visual_navigation_enabled: - if artifact_type not in self.full_360_objects.keys(): - self.full_360_objects[artifact_type] = [] - self.full_360_objects[artifact_type].append({'yaw': self.yaw, 'distance': self.scan_distance_to_obstacle}) - - def wait_for_hauler(self): - while not self.hauler_ready: - try: - self.wait(timedelta(seconds=1)) - except MoonException as e: - print(self.sim_time, self.robot_name, "Exception while waiting for hauler to arrive: ", str(e)) - self.hauler_ready = False - - def wait_for_arm(self, angle): - wait_start = self.sim_time - while self.sim_time - wait_start < timedelta(seconds=20) and (self.mount_angle is None or abs(normalizeAnglePIPI(angle - self.mount_angle)) > 0.2): - try: - self.wait(timedelta(seconds=1)) - except MoonException as e: - print(self.sim_time, self.robot_name, "Exception while waiting for arm to align: ", str(e)) - - def run(self): - - def process_volatiles(vol_string): - - vol_list_one = vol_string.split(',') - self.vol_list = [list(map(float, s.split())) for s in vol_list_one] - print (self.sim_time, self.robot_name, "Volatiles (%d): " % len(self.vol_list), self.vol_list) - - def vslam_reset_time(response): - self.vslam_reset_at = self.sim_time - - try: - self.wait_for_init() - - self.set_cam_angle(-0.05) - self.set_light_intensity("0.2") - - self.send_request('get_volatile_locations', process_volatiles) - - # move bucket to the back so that it does not interfere with lidar - # TODO: is there a better position to stash the bucket for driving? - - # move arm to the back, let robot slide off of a hill if that's where it starts - self.publish("bucket_drop", [math.pi, 'reset']) - - # point wheels downwards and wait until on flat terrain or timeout - print(self.sim_time, self.robot_name, "Letting excavator slide down the hill") - start_drifting = self.sim_time - while self.sim_time - start_drifting < timedelta(seconds=20) and (abs(self.pitch) > 0.05 or abs(self.roll) > 0.05): - try: - attitude = math.asin(math.sin(self.roll) / math.sqrt(math.sin(self.pitch)**2+math.sin(self.roll)**2)) - if self.debug: - print(self.sim_time, self.robot_name, "Vehicle attitude: %.2f" % attitude) - self.publish("desired_movement", [GO_STRAIGHT, math.degrees(attitude * 100), 0.1]) - self.wait(timedelta(milliseconds=100)) - except MoonException as e: - print(self.sim_time, self.robot_name, "Exception while waiting for excavator to settle: ", str(e)) - self.publish("desired_movement", [0, 0, 0]) - - self.wait_for_arm(math.pi) - - self.set_brakes(False) # start applying mild braking - - # initial 360 turn to find emptiest area - print(self.sim_time, self.robot_name, "Scanning 360 degrees of horizon to find widest segment with far enough free space") - self.lidar_distance_enabled = True - while True: - try: - self.turn(math.radians(30), ang_speed=0.8*self.max_angular_speed, with_stop=False, timeout=timedelta(seconds=40)) # get the turning stared - self.full_360_distances = [] - self.turn(math.radians(360), ang_speed=0.8*self.max_angular_speed, timeout=timedelta(seconds=40)) - break - except MoonException as e: - print(self.sim_time, self.robot_name, "Exception while performing initial turn: ", str(e)) - - self.lidar_distance_enabled = False - self.set_cam_angle(-0.05) - - min_distance_obj = min(self.full_360_distances, key=lambda x: x['distance']) - # if min distance in all directions is more than 5m, no need to drive anywhere - if min_distance_obj['distance'] < 5000: - - def distance_grouper(iterable): - prev = None - group = [] - for item in iterable: - if not prev or abs(item['distance'] - prev['distance']) <= 300: - group.append(item) - else: - yield group - group = [item] - prev = item - if group: - yield group - - clusters = list(enumerate(distance_grouper(self.full_360_distances))) - m = [] - for c in clusters: - sum_sin = sum([math.sin(a['yaw']) for a in c[1]]) - sum_cos = sum([math.cos(a['yaw']) for a in c[1]]) - mid_angle = math.atan2(sum_sin, sum_cos) - if median([o['distance'] for o in c[1]]) > 6000: - m.append({'yaw': mid_angle, 'wedge_width': len(c[1])}) - if len(m) > 0: - bestyawobj = max(m, key=lambda x: x['wedge_width']) - - try: - print(self.sim_time, self.robot_name, "excavator: Driving into-clear-space, yaw (internal coordinates) [%.2f]" % (normalizeAnglePIPI(bestyawobj['yaw']))) - self.turn(bestyawobj['yaw'] - self.yaw, timeout=timedelta(seconds=40)) - self.go_straight(4) - except MoonException as e: - print(self.sim_time, self.robot_name, "Exception while performing initial going away from obstacles: ", str(e)) - else: - print(self.sim_time, self.robot_name, "No direction with 6+m unobstructed area found") - else: - print(self.sim_time, self.robot_name, "More than 5m of space in all directions, no need to move") - - print(self.sim_time, self.robot_name, "Asking hauler to arrive to distance") - - self.send_request('external_command hauler_1 follow') # force hauler to come real close, this will make it lose visual match with processing plant - self.wait_for_hauler() - - - # first, turn away from hauler - # turn 360deg until seen, mark angle where the bbox was the biggest, then turn 180deg from there (want to face away as much as possible) - # then, when hauler turns towards us (and follows?), it should have the same yaw which we can then share with it - - - def angle_grouper(iterable): - prev = None - group = [] - for item in iterable: - if not prev or abs(normalizeAnglePIPI(item['yaw'] - prev['yaw'])) <= GROUP_ANGLES_MAX_DIFF: - group.append(item) - else: - yield group - group = [item] - prev = item - if group: - yield group - - print(self.sim_time, self.robot_name, "Scanning 360 degrees of horizon to find hauler (widest wedge with continuous hauler detection)") - - self.visual_navigation_enabled = True - while True: - while True: - try: - self.turn(math.radians(30), ang_speed=0.8*self.max_angular_speed, with_stop=False, timeout=timedelta(seconds=40)) # get the turning stared - self.full_360_objects = {} - self.turn(math.radians(360), ang_speed=0.8*self.max_angular_speed, with_stop=False, timeout=timedelta(seconds=40)) - if "rover" not in self.full_360_objects.keys() or len(self.full_360_objects["rover"]) == 0: - print(self.sim_time, self.robot_name, "Hauler not found during 360 turn") - else: - clusters = list(enumerate(angle_grouper(self.full_360_objects["rover"]))) - if self.debug: - print(self.sim_time, self.robot_name, "Cluster list: ", str(clusters)) - biggest_wedge_obj = max(clusters, key=lambda x: len(x[1])) - sum_sin = sum([math.sin(a['yaw']) for a in biggest_wedge_obj[1]]) - sum_cos = sum([math.cos(a['yaw']) for a in biggest_wedge_obj[1]]) - mid_angle = math.atan2(sum_sin, sum_cos) - print(self.sim_time, self.robot_name, "excavator: away-from-hauler angle (internal coordinates) [%.2f]" % (normalizeAnglePIPI(math.pi + mid_angle))) - turn_needed = normalizeAnglePIPI(math.pi + mid_angle - self.yaw) - # turn a little less to allow for extra turn after the effort was stopped - self.turn(turn_needed if turn_needed >= 0 else (turn_needed + 2*math.pi), ang_speed=0.8*self.max_angular_speed, timeout=timedelta(seconds=40)) - break - except MoonException as e: - print(self.sim_time, self.robot_name, "Exception while performing initial turn: ", str(e)) - - # TODO: this will be removed, shouldn't be needed - if "rover" not in self.full_360_objects.keys() or len(self.full_360_objects["rover"]) == 0: - # hauler was not found visually but must be close, perform random walk - min_dist_obj = min(self.full_360_distances, key=lambda x: x['distance']) - self.turn(min_dist_obj['yaw'] - self.yaw, timeout=timedelta(seconds=40)) - self.go_straight(3) - while abs(self.pitch) > 0.2: - self.publish("desired_movement", [GO_STRAIGHT, 0, self.default_effort_level]) - self.wait(timedelta(milliseconds=100)) - self.publish("desired_movement", [0, 0, 0]) - else: - break - - self.visual_navigation_enabled = False - - self.auto_light_adjustment = True - # self.set_light_intensity("0.4") - - print(self.sim_time, self.robot_name, "Resetting VSLAM, requesting true_pose") - - # this will also trigger true pose once slam starts sending data - self.send_request('vslam_reset', vslam_reset_time) - - while self.vol_list is None or not self.true_pose: - try: - self.wait(timedelta(seconds=1)) - except MoonException as e: - print(self.sim_time, self.robot_name, "Exception while waiting for true pose: ", str(e)) - - # calculate position behind the excavator to send hauler to, hauler will continue visually from there - v = np.asmatrix(np.asarray([self.xyz[0], self.xyz[1], 1])) - c, s = np.cos(self.yaw), np.sin(self.yaw) - R = np.asmatrix(np.array(((c, -s, 0), (s, c, 0), (0, 0, 1)))) - T = np.asmatrix(np.array(((1, 0, -8),(0, 1, 0),(0, 0, 1)))) - pos = np.dot(R, np.dot(T, np.dot(R.I, v.T))) - #self.send_request('external_command hauler_1 goto %.1f %.1f %.2f' % (pos[0], pos[1], self.yaw)) - - #self.send_request('external_command hauler_1 align %f' % self.yaw) # TODO: due to skidding, this yaw may still change - - self.hauler_ready = False - self.send_request('external_command hauler_1 set_yaw %f' % self.yaw) # tell hauler looking at rover - - # wait for hauler to start following, will receive osgar broadcast when done - while not self.hauler_ready: - try: - self.wait(timedelta(seconds=1)) - except MoonException as e: - print(self.sim_time, self.robot_name, "Exception while waiting for hauler to arrive: ", str(e)) - - self.hauler_ready = False - self.send_request('external_command hauler_1 approach %f' % self.yaw) - - # do not brake here, it will allow robots to align better - while not self.hauler_ready: - try: - self.wait(timedelta(seconds=3)) - except MoonException as e: - print(self.sim_time, self.robot_name, "Exception while waiting for hauler to arrive: ", str(e)) - - self.hauler_ready = False - - self.send_request('external_command hauler_1 follow') - #self.send_request('external_command hauler_1 set_yaw %f' % self.yaw) # tell hauler looking at rover is in 'yaw' direction - - # reset again in case hauler was driving in front of excavator CANT RESET IF WE MOVED (TURNED) - #self.vslam_reset_at = None - #self.send_request('vslam_reset', vslam_reset_time) - #while self.vslam_reset_at is None or self.sim_time - self.vslam_reset_at < timedelta(seconds=3): - # try: - # self.wait(timedelta(seconds=1)) - # except BusShutdownException: - # raise - # except: - # pass - - self.virtual_bumper = VirtualBumper(timedelta(seconds=3), 0.2) # radius of "stuck" area; a little more as the robot flexes - - - def pursue_volatile(): - v = self.current_volatile - wait_for_mapping = False - wait_for_hauler_requested = None - ARRIVAL_TOLERANCE = 1 # if we are within 1m of target, stop - while True: - try: - - if not self.vslam_valid: # vslam_valid will only be turned to False if hauler origin hasn't been requested yet - self.publish("desired_movement", [0,0,0]) - - # hauler approach, VSLAM should be running - self.send_request('external_command hauler_1 approach %f' % self.yaw) - self.set_brakes(True) - - # reset VSLAM (this should reset self.vslam_valid) - self.tf['vslam']['trans_matrix'] = None - self.vslam_reset_at = None - self.send_request('vslam_reset', vslam_reset_time) - while self.vslam_reset_at is None or self.sim_time - self.vslam_reset_at < timedelta(seconds=3): - try: - self.wait(timedelta(seconds=1)) - except MoonException as e: - print(self.sim_time, self.robot_name, "Exception while waiting for VSLAM reset: ", str(e)) - - while not self.hauler_ready: - try: - self.wait(timedelta(seconds=3)) - except MoonException as e: - print(self.sim_time, self.robot_name, "Exception while waiting for hauler to be ready: ", str(e)) - - self.hauler_ready = False - - self.send_request('external_command hauler_1 request_true_pose') - try: - self.wait(timedelta(seconds=5)) # wait for pose to update (TODO: wait for a flag instead) - except MoonException as e: - print(self.sim_time, self.robot_name, "Exception while waiting for true pose: ", str(e)) - - - self.set_brakes(False) - self.send_request('external_command hauler_1 follow') - - print(self.sim_time, self.robot_name, "excavator: Pursuing volatile [%.1f,%.1f]" % (v[0],v[1])) - while wait_for_mapping or (wait_for_hauler_requested is not None and self.sim_time < wait_for_hauler_requested): - self.wait(timedelta(seconds=1)) - - with LidarCollisionMonitor(self, 1500): # some distance needed not to lose tracking when seeing only obstacle up front - # turning in place probably not desirable because hauler behind may be in the way, may need to send it away first - if distance(self.xyz, v) > ARRIVAL_TOLERANCE: # only turn if we are further than 2m, if closer, it probably means there was an exception while we were already on location - angle_diff = self.get_angle_diff(v) - #self.turn(math.copysign(min(abs(angle_diff),math.pi/4),angle_diff), timeout=timedelta(seconds=15)) - # ideal position is 0.5m in front of the excavator - # TODO: fine-tune offset and tolerance given slipping - self.going_to_volatile = True - try: - self.go_to_location(v, self.default_effort_level, offset=ARRIVAL_OFFSET, full_turn=True, timeout=timedelta(minutes=5), tolerance=ARRIVAL_TOLERANCE) - except NewVolatileTargetException: - raise - finally: - self.going_to_volatile = False - return v #arrived - - except LidarCollisionException as e: #TODO: long follow of obstacle causes loss, go along under steeper angle - print(self.sim_time, self.robot_name, "Lidar") - if distance(self.xyz, v) < 1: - return v #arrived - self.inException = True - try: - self.lidar_drive_around() - except ResumeRequestedException as e: - wait_for_hauler_requested = self.sim_time + timedelta(seconds=5) # resume only after 5 seconds to give hauler chance to get closer - print(self.sim_time, self.robot_name, "Hauler wants to resume") - except WaitRequestedException as e: - self.send_speed_cmd(0.0, 0.0) - wait_for_hauler_requested = self.sim_time + timedelta(minutes=2) # will wait at most 2 minutes - print(self.sim_time, self.robot_name, "Hauler requested to wait") - except VSLAMDisabledException as e: - print(self.sim_time, self.robot_name, "VSLAM: mapping disabled, waiting") - self.send_speed_cmd(0.0, 0.0) - wait_for_mapping = True - except VSLAMEnabledException as e: - print(self.sim_time, self.robot_name, "VSLAM: mapping re-enabled") - wait_for_mapping = False - except VSLAMLostException as e: - print(self.sim_time, self.robot_name, "VSLAM lost, re-localize via hauler") - - self.inException = False - except VirtualBumperException as e: - self.send_speed_cmd(0.0, 0.0) - print(self.sim_time, self.robot_name, "Bumper") - if distance(self.xyz, v) < 1: - return v # arrived - self.inException = True - try: - self.go_straight(-1) # go 1m in opposite direction - angle_diff = self.get_angle_diff(v) - self.drive_around_rock(-math.copysign(5, angle_diff)) # assume 6m the most needed - except ResumeRequestedException as e: - wait_for_hauler_requested = self.sim_time + timedelta(seconds=5) - print(self.sim_time, self.robot_name, "Hauler wants to resume") - except WaitRequestedException as e: - self.send_speed_cmd(0.0, 0.0) - wait_for_hauler_requested = self.sim_time + timedelta(minutes=2) - print(self.sim_time, self.robot_name, "Hauler requested to wait") - except VSLAMDisabledException as e: - print(self.sim_time, self.robot_name, "VSLAM: mapping disabled, waiting") - self.send_speed_cmd(0.0, 0.0) - wait_for_mapping = True - except VSLAMEnabledException as e: - print(self.sim_time, self.robot_name, "VSLAM: mapping re-enabled") - wait_for_mapping = False - except VSLAMLostException as e: - print(self.sim_time, self.robot_name, "VSLAM lost, re-localize via hauler") - - self.inException = False - except VSLAMDisabledException as e: - print(self.sim_time, self.robot_name, "VSLAM: mapping disabled, waiting") - self.send_speed_cmd(0.0, 0.0) - wait_for_mapping = True - except VSLAMEnabledException as e: - print(self.sim_time, self.robot_name, "VSLAM: mapping re-enabled") - wait_for_mapping = False - except WaitRequestedException as e: - self.send_speed_cmd(0.0, 0.0) - wait_for_hauler_requested = self.sim_time + timedelta(minutes=2) - print(self.sim_time, self.robot_name, "Hauler requested to wait") - except ResumeRequestedException as e: - wait_for_hauler_requested = self.sim_time + timedelta(seconds=5) - print(self.sim_time, self.robot_name, "Hauler wants to resume") - except VSLAMLostException as e: - print(self.sim_time, self.robot_name, "VSLAM lost, re-localize via hauler") - - - - def reposition(mount_angle, distal_angle): - print(self.sim_time, self.robot_name, "Adjusting digging position: mount angle: %.2f, distal angle: %.2f" % (normalizeAnglePIPI(mount_angle), distal_angle)) - # if found an angle of a volatile but it's too far, come closer before digging - # if distal arm was negative during volatile detection, volatile is away from rover; otherwise it is under the rover - self.publish("bucket_drop", [math.pi, 'reset']) # first, stash arm and wait for it so that visual navigation doesn't confuse hauler - start_alignment = self.sim_time - while self.sim_time - start_alignment < timedelta(seconds=20) and self.mount_angle is None or abs(normalizeAnglePIPI(math.pi - self.mount_angle)) > 0.2: - try: - self.wait(timedelta(seconds=1)) - except MoonException as e: - print(self.sim_time, self.robot_name, "Exception while waiting for arm to align: ", str(e)) - - self.send_request('external_command hauler_1 onhold') - try: - self.wait(timedelta(seconds=8)) - except MoonException as e: - print(self.sim_time, self.robot_name, "Exception while waiting for hauler to assume position: ", str(e)) - - - start_pose = self.xyz - start_time = self.sim_time - effort = self.default_effort_level/2 if distal_angle < DISTAL_THRESHOLD else -self.default_effort_level/2 - if abs(normalizeAnglePIPI(mount_angle)) < math.pi/2: - self.publish("desired_movement", [GO_STRAIGHT, int(100*math.degrees(normalizeAnglePIPI(mount_angle))), effort]) - else: - self.publish("desired_movement", [ - GO_STRAIGHT, - int(100*math.degrees(normalizeAnglePIPI(math.copysign(math.pi - abs(normalizeAnglePIPI(mount_angle)), -normalizeAnglePIPI(mount_angle))))), - -effort]) - - while distance(start_pose, self.xyz) < 0.7: # go 0.7m closer to volatile - try: - self.wait(timedelta(milliseconds=100)) - except MoonException as e: - print(self.sim_time, self.robot_name, "Exception while adjusting position: ", str(e)) - traceback.print_exc(file=sys.stdout) - - - self.publish("desired_movement", [0,0,0]) - try: - self.wait(timedelta(seconds=3)) - except MoonException as e: - print(self.sim_time, self.robot_name, "Exception while waiting to come to stop: ", str(e)) - - #self.send_request('external_command hauler_1 approach %f' % normalizeAnglePIPI(self.yaw + mount_angle)) - - #self.hauler_ready = False - #while not self.hauler_ready: - # try: - # self.wait(timedelta(seconds=1)) - # except BusShutdownException: - # raise - # except: - # pass - # end of reposition function - - while len(self.vol_list) > 0: - self.current_volatile = self.get_next_volatile() - if self.current_volatile is None: - break - while True: - try: - pursue_volatile() - except NewVolatileTargetException: - continue - break - v = self.current_volatile - self.current_volatile = None - - accum = 0 # to check if we picked up 100.0 total and can finish - for attempt_offset in [{'dist': 0.0, 'start_dig_angle': 0.0}, {'dist': 4, 'start_dig_angle': math.pi}, {'dist': -8, 'start_dig_angle': 0.0}]: - starting_point = self.xyz - starting_time = self.sim_time - while abs(abs(attempt_offset['dist']) - distance(self.xyz, starting_point)) > 1 and self.sim_time - starting_time < timedelta(seconds=15): - try: - self.go_straight(attempt_offset['dist'] - math.copysign(distance(self.xyz, starting_point), attempt_offset['dist'])) - except MoonException as e: - # excess pitch or other exception could happen but we are stopped so should stabilize - print(self.sim_time, self.robot_name, "Exception when adjusting linearly", str(e)) - - self.send_speed_cmd(0.0, 0.0) - - print ("---- VOLATILE REACHED ----") - - try: - self.wait(timedelta(seconds=2)) - except MoonException as e: - print(self.sim_time, self.robot_name, "Exception while waiting to come to stop: ", str(e)) - - # TODO: could look for volatile while waiting - # wait for hauler to start following, will receive osgar broadcast when done - #while not self.hauler_ready: - # try: - # self.wait(timedelta(seconds=1)) - # except BusShutdownException: - # raise - # except: - # # excess pitch or other exception could happen but we are stopped so should stabilize - # pass - - #self.volatile_reached = False # TEST - #self.send_request('external_command hauler_1 backout') # Testing - #continue # testing - - self.send_request('external_command hauler_1 onhold') # re-follow after each 0,+2,-2 attempt - - found_angle = None - - # mount 0.4-1.25 or 1.85-2.75 means we can't reach under the vehicle because of the wheels - mount_angle_sequence = [0.0, 0.39, -0.39, -0.83, 0.83, 1.26, -1.26, -1.55, 1.55, 1.84, -1.84, 2.31, -2.31, -2.76, 2.76, 3.14] - for a in mount_angle_sequence: - self.publish("bucket_dig", [a + attempt_offset['start_dig_angle'], 'append']) - - while found_angle is None: - best_mount_angle = None - best_angle_volume = 0 - best_distal_angle = None - - self.set_brakes(True) - exit_time = self.sim_time + timedelta(seconds=250) - while found_angle is None and self.sim_time < exit_time: - # TODO instead of/in addition to timeout, trigger exit by bucket reaching reset position - try: - self.wait(timedelta(milliseconds=100)) - except MoonException as e: - print(self.sim_time, self.robot_name, "Exception while waiting in loop: ", str(e)) - - if self.volatile_dug_up[1] != 100: - # we found volatile - if best_mount_angle is None: - nma = normalizeAnglePIPI(self.mount_angle) - if -math.pi/2 <= nma <= math.pi/2: - # if mount angle is in the front half of the vehicle, approach directly from the back - approach_angle = 0.0 - elif nma < -math.pi/2: - approach_angle = normalizeAnglePIPI(nma + math.pi/2) # rover clockwise 90 degrees - elif nma > math.pi/2: - approach_angle = normalizeAnglePIPI(nma - math.pi/2) # rover counterclockwise 90 degrees - - # point arm in the hauler approach direction so that hauler aligns with the center of the excavator - self.publish("bucket_dig", [normalizeAnglePIPI(approach_angle + math.pi), 'standby']) # lift arm and clear rest of queue - self.wait_for_arm(normalizeAnglePIPI(approach_angle + math.pi)) - - self.hauler_ready = False - self.send_request('external_command hauler_1 approach %f' % normalizeAnglePIPI(self.yaw + approach_angle)) - - # NOTE: this approach may bump excavator, scoop volume may worsen - while not self.hauler_ready: - try: - self.wait(timedelta(seconds=1)) - except MoonException as e: - print(self.sim_time, self.robot_name, "Exception while waiting for hauler to arrive: ", str(e)) - self.hauler_ready = False - - - # wait for hauler to arrive, get its yaw, then we know which direction to drop - drop_angle = normalizeAnglePIPI((self.hauler_yaw - self.yaw) - math.pi) - print(self.sim_time, self.robot_name, "Drop angle set to: %.2f" % drop_angle) - - exit_time = self.sim_time + timedelta(seconds=80) - self.publish("bucket_drop", [drop_angle, 'append']) # this will be duplicated below but need to queue now or load would be lost by another dig happening first - self.publish("bucket_dig", [nma - 0.3, 'append']) - self.publish("bucket_dig", [nma + 0.3, 'append']) - # TODO: this plan doesn't end once these moves are executed, only with timeout - best_distal_angle = self.first_distal_angle - best_angle_volume = self.volatile_dug_up[2] - best_mount_angle = nma - - elif self.volatile_dug_up[2] > best_angle_volume: - best_distal_angle = self.first_distal_angle - best_angle_volume = self.volatile_dug_up[2] - best_mount_angle = self.mount_angle - - accum += self.volatile_dug_up[2] - if self.volatile_dug_up[2] > DIG_GOOD_LOCATION_MASS: - found_angle = best_mount_angle - # go for volatile drop (to hauler), wait until finished - self.publish("bucket_drop", [drop_angle, 'prepend']) - while self.volatile_dug_up[1] != 100: - try: - self.wait(timedelta(milliseconds=300)) - except MoonException as e: - print(self.sim_time, self.robot_name, "Exception while waiting to empty bin: ", str(e)) - self.first_distal_angle = None - - self.set_brakes(False) - - if found_angle is None and best_mount_angle is not None: - # didn't find good position (timeout) but found something - reposition and try again - reposition(best_mount_angle, best_distal_angle) - # after repositioning, try again the whole circle starting with best angle first - for a in mount_angle_sequence: - self.publish("bucket_dig", [normalizeAnglePIPI(best_mount_angle + a), 'append']) - else: # found good position or found nothing at all - break - - def scoop_all(angle): - nonlocal accum - while True: - dig_start = self.sim_time - self.publish("bucket_dig", [angle, 'reset']) - while self.volatile_dug_up[1] == 100: - try: - self.wait(timedelta(milliseconds=300)) - except MoonException as e: - print(self.sim_time, self.robot_name, "Exception while waiting to detect volatile: ", str(e)) - if self.sim_time - dig_start > timedelta(seconds=120): # TODO: timeout needs to be adjusted to the ultimate digging plan - # move bucket out of the way and continue to next volatile - if self.debug: - print(self.sim_time, self.robot_name, "Arm movement timed out or nothing to dig, terminating scooping") - self.publish("bucket_drop", [drop_angle, 'append']) - return False - accum += self.volatile_dug_up[2] - self.publish("bucket_drop", [drop_angle, 'append']) - while self.volatile_dug_up[1] != 100: - try: - self.wait(timedelta(milliseconds=300)) - except MoonException as e: - print(self.sim_time, self.robot_name, "Exception while waiting to empty bin: ", str(e)) - - if abs(accum - 100.0) < 0.0001: - print(self.sim_time, self.robot_name, "Volatile amount %.2f transfered, terminating" % accum) - # TODO: adjust transformation matrix using real location of the volatile - # TODO: assumes 100 to be total volatile at location, actual total reported initially, parse and match instead - return True - - full_success = False - if found_angle is not None: - # have best position, dig everything up, adjust position, move to next volatile - self.set_brakes(True) - if scoop_all(found_angle): - self.vol_list.remove(v)# once scooping finished or given up on, remove volatile from list - - v = np.asmatrix(np.asarray([v[0], v[1], 1])) - c, s = np.cos(self.yaw), np.sin(self.yaw) - Rr = np.asmatrix(np.array(((c, -s, 0), (s, c, 0), (0, 0, 1)))) - - # now in coord system WRT robot - T = np.asmatrix(np.array(((1, 0, -TYPICAL_VOLATILE_DISTANCE),(0, 1, 0),(0, 0, 1)))) - c, s = np.cos(best_mount_angle), np.sin(best_mount_angle) - Rv = np.asmatrix(np.array(((c, -s, 0), (s, c, 0), (0, 0, 1)))) - - true_pos = np.dot(Rr, np.dot(Rv, np.dot(T, np.dot(Rv.I, np.dot(Rr.I, v.T))))) - - m = translationToMatrix([float(true_pos[0])-self.xyz[0], float(true_pos[1])-self.xyz[1], 0.0]) - print(self.sim_time, self.robot_name, "Adjusting XY based on volatile location: from [%.1f,%.1f] to [%.1f,%.1f]" % (self.xyz[0], self.xyz[1], float(true_pos[0]), float(true_pos[1]))) - self.tf['vslam']['trans_matrix'] = np.dot(m, self.tf['vslam']['trans_matrix']) - full_success = True - self.set_brakes(False) - - - # wait for bucket to be dropped and/or position reset before moving on - self.publish("bucket_drop", [math.pi, 'reset']) - while self.volatile_dug_up[1] != 100 or (self.mount_angle is None or abs(normalizeAnglePIPI(math.pi - self.mount_angle)) > 0.2): - try: - self.wait(timedelta(seconds=1)) - except MoonException as e: - print(self.sim_time, self.robot_name, "Exception while waiting to empty bin: ", str(e)) - - if full_success: - break # do not de-attempt 2m back and 2m front - - # otherwise re-follow after giving up on this position - self.send_request('external_command hauler_1 follow') - # end of for(0,+2,-4) - - if accum == 0 and not self.hauler_pose_requested: - self.set_brakes(True) - - self.send_request('external_command hauler_1 approach %f' % self.hauler_yaw) - while not self.hauler_ready: - try: - self.wait(timedelta(seconds=1)) - except MoonException as e: - print(self.sim_time, self.robot_name, "Exception while waiting for hauler to arrive: ", str(e)) - - self.hauler_ready = False - - self.send_request('external_command hauler_1 request_true_pose') - try: - self.wait(timedelta(seconds=5)) # wait for pose to update (TODO: wait for a flag instead) - except MoonException as e: - print(self.sim_time, self.robot_name, "Exception while waiting to true pose: ", str(e)) - - self.set_brakes(False) - - self.send_request('external_command hauler_1 follow') # re-follow after each volatile - - - print("***************** VOLATILES EXHAUSTED ****************************") - - - except BusShutdownException: - pass - - print ("EXCAVATOR END") - -# vim: expandtab sw=4 ts=4 diff --git a/moon/controller_hauler_round2.py b/moon/controller_hauler_round2.py deleted file mode 100644 index fab69d1e8..000000000 --- a/moon/controller_hauler_round2.py +++ /dev/null @@ -1,579 +0,0 @@ -""" - Space Robotics Challenge 2 -""" - -from scipy import spatial -import numpy as np -import math -from datetime import timedelta -from statistics import median -import traceback -import sys - -from osgar.bus import BusShutdownException - -from moon.controller import ps, SpaceRoboticsChallenge, MoonException, ChangeDriverException, VirtualBumperException, min_dist, LidarCollisionException, LidarCollisionMonitor -from osgar.lib.quaternion import euler_zyx -from osgar.lib.virtual_bumper import VirtualBumper -from osgar.lib.mathex import normalizeAnglePIPI -from moon.moonnode import CAMERA_WIDTH, CAMERA_HEIGHT, CAMERA_FOCAL_LENGTH, CAMERA_BASELINE - -TURN_ON = 12 # radius of circle when turning -GO_STRAIGHT = float("inf") -EXCAVATOR_DRIVING_GAP = 1.8 # can't be further or every bump will look like the excavator on lidar given the camera must be tilted down so that rover is visible up close -EXCAVATOR_ONHOLD_GAP = 3 # can't be further or every bump will look like the excavator on lidar given the camera must be tilted down so that rover is visible up close -EXCAVATOR_DIGGING_GAP = 0.55 # we should see the arm in the middle, not the back of the rover -DISTANCE_TOLERANCE = 0.3 - -class ExcavatorLostException(MoonException): - pass - -class SpaceRoboticsChallengeHaulerRound2(SpaceRoboticsChallenge): - def __init__(self, config, bus): - super().__init__(config, bus) - bus.register("desired_movement") - self.robot_name = "hauler_1" - - self.rover_angle = 0.0 - self.use_gimbal = True - self.vslam_reset_at = None - self.goto = None - self.finish_visually = False - self.arrived_message_sent = False - self.objects_in_view = {} - self.bin_content = None - self.excavator_yaw = None - self.default_effort_level = 1100 - self.rover_distance = 15 - self.min_scan_distance = 15 - self.scan_driving = False - self.cam_lowered = False - self.driving_mode = None - self.aligned_at = None - self.target_excavator_distance = EXCAVATOR_DRIVING_GAP - self.scan_driving_phase = "yaw" - self.scan_driving_phase_start = None - self.excavator_waiting = False - self.turnto_angle = None - self.arrival_send_requested_at = None - self.set_yaw = None - self.full_360_objects = {} - self.excavator_waiting_start = None - - def vslam_reset_time(self, response): - self.vslam_reset_at = self.sim_time - - def on_osgar_broadcast(self, data): - - message_target = data.split(" ")[0] - if message_target == self.robot_name: - print(self.sim_time, self.robot_name, "Received external_command: %s" % str(data)) - command = data.split(" ")[1] - self.driving_mode = command - self.set_brakes(False) - - if command == "goto": - # x, y, heading - self.goto = [float(n) for n in data.split(" ")[2:5]] - self.send_request('vslam_reset', self.vslam_reset_time) - elif command == "approach": - self.arrived_message_sent = False - self.excavator_yaw = float(data.split(" ")[2]) - elif command == "align": - self.target_excavator_distance = EXCAVATOR_DRIVING_GAP - self.arrived_message_sent = False - self.excavator_yaw = float(data.split(" ")[2]) - elif command == "follow": - self.target_excavator_distance = EXCAVATOR_DRIVING_GAP - self.arrived_message_sent = False - elif command == "onhold": - self.target_excavator_distance = EXCAVATOR_ONHOLD_GAP - self.arrived_message_sent = False - elif command == "request_true_pose": - def forward_true_pose(result): - if result.split()[0] == 'origin': - origin = [float(x) for x in result.split()[1:]] - initial_quat = origin[3:] - initial_rpy = euler_zyx(initial_quat) # note: this is not in roll, pitch, yaw order - self.yaw_offset = self.yaw + self.yaw_offset - initial_rpy[0] - self.send_request('external_command excavator_1 hauler_true_pose %s' % result) - self.send_request('request_origin', forward_true_pose) - elif command == "turnto": - self.turnto_angle = float(data.split(" ")[2]) - elif command == "set_yaw": - self.set_yaw = float(data.split(" ")[2]) - else: - print(self.sim_time, self.robot_name, "Invalid broadcast command") - - def get_extra_status(self): - return "Bin: " + str([[a,b] for a,b in zip(self.bin_content[0], self.bin_content[1])]) - - def on_bin_info(self, data): - self.bin_content = data - - def look_for_rover(self, direction=1): - print(self.sim_time, self.robot_name, "Starting to look for excavator") - while True: - try: - with LidarCollisionMonitor(self): - self.turn(math.copysign(math.radians(self.rand.randrange(90,270)), direction), timeout=timedelta(seconds=20)) - self.turn(math.radians(360), timeout=timedelta(seconds=40)) - self.go_straight(20.0, timeout=timedelta(minutes=2)) - except ChangeDriverException as e: - print(self.sim_time, self.robot_name, "Excavator found, following") - self.publish("desired_movement", [0, 0, 0]) - raise - except (VirtualBumperException, LidarCollisionException) as e: - self.inException = True - print(self.sim_time, self.robot_name, "Lidar or Virtual Bumper!") - try: - self.go_straight(-3, timeout=timedelta(seconds=20)) - deg_angle = self.rand.randrange(-180, -90) - self.turn(math.radians(deg_angle), timeout=timedelta(seconds=10)) - except: - self.publish("desired_movement", [0, 0, 0]) - self.inException = False - except MoonException as e: - print(self.sim_time, self.robot_name, "MoonException while looking for rover, restarting turns") - traceback.print_exc(file=sys.stdout) - - - - def run(self): - - try: - self.wait_for_init() - #self.wait(timedelta(seconds=5)) - self.set_light_intensity("0.2") - self.set_cam_angle(-0.1) - self.use_gimbal = True - - self.set_brakes(True) - if False: - # point wheels downwards and wait until on flat terrain or timeout - start_drifting = self.sim_time - while self.sim_time - start_drifting < timedelta(seconds=20) and (abs(self.pitch) > 0.05 or abs(self.roll) > 0.05): - try: - attitude = math.asin(math.sin(self.roll) / math.sqrt(math.sin(self.pitch)**2+math.sin(self.roll)**2)) - if self.debug: - print(self.sim_time, self.robot_name, "Vehicle attitude: %.2f" % attitude) - self.publish("desired_movement", [GO_STRAIGHT, math.degrees(attitude * 100), 0.1]) - self.wait(timedelta(milliseconds=100)) - except MoonException as e: - print(self.sim_time, self.robot_name, "Exception while waiting for excavator to settle: ", str(e)) - self.publish("desired_movement", [0, 0, 0]) - - while True: - while self.driving_mode is None: # waiting for align command - try: - self.wait(timedelta(seconds=1)) - except MoonException as e: - print(self.sim_time, self.robot_name, "Exception while waiting to be invited to look for excavator", str(e)) - break - - self.set_brakes(False) - self.finish_visually = True - - # FIND EXCAVATOR - while True: - try: - self.excavator_waiting = True - with LidarCollisionMonitor(self): - while True: - self.turn(math.radians(self.rand.randrange(90,270)), timeout=timedelta(seconds=20)) - self.turn(math.radians(360), timeout=timedelta(seconds=40)) - self.go_straight(20.0, timeout=timedelta(minutes=2)) - except ChangeDriverException as e: - print(self.sim_time, self.robot_name, "Excavator found, following") - self.publish("desired_movement", [0, 0, 0]) - except (VirtualBumperException, LidarCollisionException) as e: - self.inException = True - print(self.sim_time, self.robot_name, "Lidar or Virtual Bumper!") - try: - self.go_straight(-3, timeout=timedelta(seconds=20)) - deg_angle = self.rand.randrange(-180, -90) - self.turn(math.radians(deg_angle), timeout=timedelta(seconds=10)) - except: - self.publish("desired_movement", [0, 0, 0]) - self.inException = False - continue - except MoonException as e: - print(self.sim_time, self.robot_name, "MoonException while looking for rover, restarting turns") - traceback.print_exc(file=sys.stdout) - continue - - # follow to ONHOLD distance - self.driving_mode = "onhold" - self.target_excavator_distance = EXCAVATOR_ONHOLD_GAP - - # driving towards excavator, wait until hauler is in position - while abs(self.target_excavator_distance - self.rover_distance) > DISTANCE_TOLERANCE: - try: - self.wait(timedelta(milliseconds=200)) - except ExcavatorLostException: - print(self.sim_time, self.robot_name, "Excavator lost while waiting to reach desired location, starting to look again") - break # look again - except MoonException: - print(self.sim_time, self.robot_name, "Exception while waiting to reach desired location, waiting on") - - if abs(self.target_excavator_distance - self.rover_distance) <= DISTANCE_TOLERANCE: - break # distance reached - - self.publish("desired_movement", [0, 0, 0]) - self.set_brakes(True) - print(self.sim_time, self.robot_name, "Sending arrived message to excavator") - self.arrived_message_sent = True - self.send_request('external_command excavator_1 arrived %.2f' % 0.0) - - # turn lights off while waiting for excavator to rotate away from hauler - # prevents potentially illuminating nearby processing plant too much and causing mis-detections - self.set_light_intensity("0.0") - while self.set_yaw is None: - try: - self.wait(timedelta(seconds=1)) - except MoonException: - print(self.sim_time, self.robot_name, "Exception while waiting for yaw alignment readiness, waiting on") - - self.set_brakes(False) - self.set_light_intensity("0.2") - - print(self.sim_time, self.robot_name, "Sending arrived message to excavator") - self.send_request('external_command excavator_1 arrived %.2f' % self.set_yaw) - #self.set_yaw = None - - - # self.finish_visually = True - - # self.excavator_waiting = True - - self.virtual_bumper = VirtualBumper(timedelta(seconds=30), 0.1) # TODO: need generous timeout because turning around or sideways moves are not sensed by bumper - - # 3 modes: 1) going to location 2) following rover visually 3) waiting for instructions to go to location - - while True: - - - if self.goto is not None: - try: - with LidarCollisionMonitor(self, 1000): - angle_diff = self.get_angle_diff(self.goto, 1) - self.turn(angle_diff, timeout=timedelta(seconds=40)) - self.go_to_location(self.goto, self.default_effort_level, full_turn=False, avoid_obstacles_close_to_destination=True, timeout=timedelta(minutes=2)) - self.turn(normalizeAnglePIPI(self.goto[2] - self.yaw), timeout=timedelta(seconds=40)) - self.send_request('external_command excavator_1 arrived %.2f' % self.yaw) - self.goto = None - self.finish_visually = True - self.set_cam_angle(-0.1) - self.set_light_intensity("0.2") - - except ChangeDriverException as e: - print(self.sim_time, self.robot_name, "Driver changed during goto?") - except LidarCollisionException as e: #TODO: long follow of obstacle causes loss, go along under steeper angle - print(self.sim_time, self.robot_name, "Lidar") - self.inException = True - self.lidar_drive_around() - self.inException = False - continue - except VirtualBumperException as e: - self.send_speed_cmd(0.0, 0.0) - print(self.sim_time, self.robot_name, "Bumper") - self.inException = True - self.go_straight(-1) # go 1m in opposite direction - self.drive_around_rock(math.copysign(4, 1 if self.rand.getrandbits(1) == 0 else -1)) # assume 6m the most needed - self.inException = False - continue - - try: - if self.excavator_waiting: - self.turn(math.copysign(math.radians(self.rand.randrange(90,270)), self.rover_angle), timeout=timedelta(seconds=20)) - self.turn(math.radians(360), timeout=timedelta(seconds=40)) - self.go_straight(20.0, timeout=timedelta(seconds=40)) - else: - if self.rover_distance > 5 and self.driving_mode != "approach": - with LidarCollisionMonitor(self, 1000): - self.wait(timedelta(seconds=1)) - else: - self.wait(timedelta(seconds=1)) - except LidarCollisionException as e: #TODO: long follow of obstacle causes loss, go along under steeper angle - print(self.sim_time, self.robot_name, "Lidar while following/waiting, distance: %.1f" % self.rover_distance) - self.excavator_waiting = True - self.send_request('external_command excavator_1 wait') - self.inException = True - - try: - self.lidar_drive_around() # TODO: if we lose visual of excavator, will keep going and never find, maybe turn 360 - except ExcavatorLostException as e: - self.excavator_waiting = True - self.send_request('external_command excavator_1 wait') - except MoonException as e: - print(self.sim_time, self.robot_name, "Exception while handling Lidar", str(e)) - traceback.print_exc(file=sys.stdout) - - self.inException = False - self.send_speed_cmd(0.0, 0.0) - except ChangeDriverException as e: - print(self.sim_time, self.robot_name, "Excavator found, continuing visual driving") - except VirtualBumperException as e: # if bumper while following (e.g, blocked by a rock) - if self.arrived_message_sent: # if while waiting for loading, ignore - continue - self.send_speed_cmd(0.0, 0.0) - print(self.sim_time, self.robot_name, "Bumper") - self.inException = True - - try: - self.go_straight(-1) # go 1m in opposite direction - self.drive_around_rock(math.copysign(4, 1 if self.rand.getrandbits(1) == 0 else -1)) # assume 6m the most needed - except ExcavatorLostException as e: - self.excavator_waiting = True - self.send_request('external_command excavator_1 wait') - - self.inException = False - except ExcavatorLostException as e: - self.excavator_waiting = True - self.send_request('external_command excavator_1 wait') - - except BusShutdownException: - pass - - print("HAULER END") - - def on_vslam_pose(self, data): - super().on_vslam_pose(data) - - if self.sim_time is None or self.last_position is None or self.yaw is None: - return - - if self.vslam_reset_at is not None and self.sim_time - self.vslam_reset_at > timedelta(seconds=3) and not math.isnan(data[0][0]) and self.tf['vslam']['trans_matrix'] is None: - # request origin and start tracking in correct coordinates as soon as first mapping lock occurs - # TODO: another pose may arrive while this request is still being processed (not a big deal, just a ROS error message) - self.send_request('request_origin', self.register_origin) - - - def on_artf(self, data): - - artifact_type = data[0] - center_x = data[1] + data[3] / 2 - center_y = data[2] + data[4] / 2 - img_x1 = data[1] - img_x2 = data[1] + data[3] - nr_pixels = data[5] - artf_distance = data[6] # in meters - - if self.sim_time is None: - return - - - #if self.set_yaw is not None: - # if artifact_type not in self.full_360_objects.keys(): - # self.full_360_objects[artifact_type] = [] - # self.full_360_objects[artifact_type].append(self.yaw) - - if not self.finish_visually: - return - - self.objects_in_view[artifact_type] = { - "expiration": self.sim_time + timedelta(milliseconds=1000), - "distance": artf_distance - } - - if artifact_type == "rover" or artifact_type == "excavator_arm": - if self.excavator_waiting_start is None: - self.excavator_waiting_start = self.sim_time - - # do not act on the first matching frame, keep moving in the same manner a bit longer to get the desired view more established - if self.excavator_waiting and self.sim_time - self.excavator_waiting_start > timedelta(milliseconds=800): - self.send_request('external_command excavator_1 resume') - self.excavator_waiting = False - self.excavator_waiting_start = None - raise ChangeDriverException(data) - - # TODO: maybe just use min of stereo cam distances? - if artf_distance < 1.3: - self.rover_distance = self.min_scan_distance # for stereo camerra distance less than 1.3m, rely on lidar for distance - elif len(self.median_scan) > 0: - self.rover_distance = max(min(self.median_scan)/1000.0, min([self.objects_in_view[a]['distance'] for a in self.objects_in_view])) # TODO: ????? - - if artifact_type == "excavator_arm" or (artifact_type == "rover" and "excavator_arm" not in self.objects_in_view.keys()): - screen_x1 = (CAMERA_WIDTH / 2 - img_x1) - screen_x2 = (CAMERA_WIDTH / 2 - img_x2) - angle_x1 = math.atan( screen_x1 / float(CAMERA_FOCAL_LENGTH)) - angle_x2 = math.atan( screen_x2 / float(CAMERA_FOCAL_LENGTH)) - angle_offset = math.atan( (CAMERA_BASELINE / 2) / (self.rover_distance + 1.1)) - self.rover_angle = (angle_x1 + angle_x2) / 2 + angle_offset - - -# if (self.debug): -# print (self.sim_time, self.robot_name, "Rover angle: %.2f dist: %d" % (angle_x, self.rover_distance)) - - - if not self.inException and not self.arrived_message_sent and not self.scan_driving: - if self.rover_distance > 6 and self.scan_distance_to_obstacle < 4000: - turn = self.get_avoidance_turn() - self.publish("desired_movement", [turn, 0, self.default_effort_level]) - if (self.debug): - print(self.sim_time, self.robot_name, "Visual drive: avoiding obstacle, obstacle dist: %d, rover dist: %.1f" % (self.scan_distance_to_obstacle, self.rover_distance)) - elif ( - artifact_type == "excavator_arm" or (artifact_type == "rover" and "excavator_arm" not in self.objects_in_view.keys()) - ): - # drive visually until both in 'arrival' mode and at target distance - self.last_rover_timestamp = self.sim_time - - direction = 1 if self.target_excavator_distance < self.rover_distance else -1 - - # if hauler is within 1m of something (presumably excavator), regardless of target distance from excavator, move sideways to center - # if bbox center in left or right quarter, turn in place - # else if bbox off center turn while moving - # else (bbox in center) go straight - - # TODO: if hauler approaches on a diagonal, it may not get close enough to switch to lidar driving - speed = self.default_effort_level - if self.set_yaw is None: # skip adjustments before the yaw is set, need to hit excavator with a little force to align yaws accurately - if self.rover_distance < 3: - speed = 0.5 * self.default_effort_level - elif self.rover_distance < self.target_excavator_distance + 2: - speed = 0.5 * self.default_effort_level - elif self.rover_distance > self.target_excavator_distance + 3: - speed = 1.2 * self.default_effort_level - - # if too close, just go straight back first - if self.driving_mode != "approach" and abs(self.target_excavator_distance - self.rover_distance) < DISTANCE_TOLERANCE: - # on approach, tolerance is not good enough, needs to be close than; pause and conclusion of approach will be performed by scan_driving - self.publish("desired_movement", [0, 0, 0]) - if (self.debug): - print(self.sim_time, self.robot_name, "Visual drive: within range, pausing, rover distance: %d,%.1f" % (self.scan_distance_to_obstacle, self.rover_distance)) - - elif self.target_excavator_distance > self.rover_distance: - self.publish("desired_movement", [GO_STRAIGHT, 0, -speed]) - if (self.debug): - print(self.sim_time, self.robot_name, "Visual drive: too close, going straight back, rover distance: %d,%.1f" % (self.scan_distance_to_obstacle, self.rover_distance)) - - elif center_x < CAMERA_WIDTH/4: - if (self.debug): - print(self.sim_time, self.robot_name, "Visual drive: Turning sharply left, obstacle dist: %d, rover dist: %.1f" % (self.scan_distance_to_obstacle, self.rover_distance)) - self.publish("desired_movement", [direction * TURN_ON/3, 0, direction * speed]) - elif center_x < (CAMERA_WIDTH/2 - 20): - if (self.debug): - print(self.sim_time, self.robot_name, "Visual drive: steering left, rover distance: %d,%.1f" % (self.scan_distance_to_obstacle, self.rover_distance)) - self.publish("desired_movement", [direction * TURN_ON, 0, direction * speed]) - elif center_x > 3*CAMERA_WIDTH/4: - if (self.debug): - print(self.sim_time, self.robot_name, "Visual drive: Turning sharply right, rover distance: %d,%.1f" % (self.scan_distance_to_obstacle, self.rover_distance)) - self.publish("desired_movement", [direction * -TURN_ON/3, 0, direction * speed]) - elif center_x > (CAMERA_WIDTH/2 + 20): - if (self.debug): - print(self.sim_time, self.robot_name, "Visual drive: steering right, rover distance: %d,%.1f" % (self.scan_distance_to_obstacle, self.rover_distance)) - self.publish("desired_movement", [direction * -TURN_ON, 0, direction * speed]) - else: - if (self.debug): - print(self.sim_time, self.robot_name, "Visual drive: going straight, rover distance: %d,%.1f" % (self.scan_distance_to_obstacle, self.rover_distance)) - self.publish("desired_movement", [GO_STRAIGHT, 0, direction * speed]) - - - def on_scan(self, data): - assert len(data) == 180 - super().on_scan(data) - - if len(self.median_scan) > 0: - self.min_scan_distance = min(self.median_scan) / 1000.0 - - delete_in_view = [artf for artf in self.objects_in_view if self.objects_in_view[artf]['expiration'] < self.sim_time] - for artf in delete_in_view: - del self.objects_in_view[artf] - - if "rover" not in self.objects_in_view.keys() and "excavator_arm" not in self.objects_in_view.keys() and self.rover_distance < 15: - self.rover_distance = 15 - if (self.debug): - print(self.sim_time, self.robot_name, "Expiring rover view and resetting distance to 15m") - if not self.arrived_message_sent: - raise ExcavatorLostException() - - if not self.finish_visually: - return - - # once distance reached, go on another 200ms to get real tight - if self.arrival_send_requested_at is not None and self.sim_time - self.arrival_send_requested_at > timedelta(milliseconds=200): - self.publish("desired_movement", [0, 0, 0]) - - if self.arrival_send_requested_at is not None and self.sim_time - self.arrival_send_requested_at > timedelta(milliseconds=1500): - self.arrival_send_requested_at = None - if self.set_yaw is not None: - print(self.sim_time, self.robot_name, "Internal yaw: %.2f, set yaw: %.2f, yaw offset: %.2f" % (self.yaw, self.set_yaw, self.yaw - self.set_yaw)) - self.yaw_offset = self.yaw - self.set_yaw - self.set_yaw = None - print(self.sim_time, self.robot_name, "Sending arrived message to excavator") - self.send_request('external_command excavator_1 arrived %.2f' % self.yaw) - - - # TODO: avoid obstacles when following (too tight turns) - - if ('rover' in self.objects_in_view.keys() or 'excavator_arm' in self.objects_in_view.keys()) and (self.driving_mode == "approach" or self.driving_mode == "align" or self.driving_mode == "turnto") and not self.arrived_message_sent and not self.inException: - # TODO: sometimes we are near rover but don't see its distance - # other times, we see a rover and its distance but stuck behind a rock - if self.excavator_yaw is None: # within requested distance and no outstanding yaw, report - if min(self.rover_distance, self.min_scan_distance) < self.target_excavator_distance: - self.set_brakes(True) - self.arrival_send_requested_at = self.sim_time - self.arrived_message_sent = True - self.excavator_yaw = None - self.scan_driving = False - if self.debug: - print(self.sim_time, self.robot_name, "Arrival handling: distance %.1f, yaw: %.2f, rover angle: %.2f; stopping" % (self.rover_distance, self.yaw, self.rover_angle)) - - if self.scan_driving or (abs(self.rover_distance - self.target_excavator_distance) < DISTANCE_TOLERANCE and self.excavator_yaw is not None): # within 1m from target, start adjusting angles - if not self.scan_driving: - if self.set_yaw is None: # do not stop when doing initial excavator approach to bump and align better - self.publish("desired_movement", [0, 0, 0]) - self.scan_driving = True - - diff = normalizeAnglePIPI(self.yaw - self.excavator_yaw) - if (self.debug): - print(self.sim_time, self.robot_name, "Arrival handling: yaw difference: (%.2f - %.2f)= %.2f, rover angle: %.2f" % (self.yaw, self.excavator_yaw, diff, self.rover_angle)) - - - if self.scan_driving_phase == "yaw": - if self.scan_driving_phase_start is None: - self.scan_driving_phase_start = self.sim_time - if abs(diff) > 0.12 and self.set_yaw is None and self.sim_time - self.scan_driving_phase_start < timedelta(seconds=30): # 10deg, don't bother otherwise; also, don't attempt to align before true pose is known - if (self.debug): - print(self.sim_time, self.robot_name, "Arrival handling: moving on a curve") - self.publish("desired_movement", [-8, -9000, math.copysign(0.5 * self.default_effort_level, diff)]) - #TODO: keep doing until diff=0; however we are bound to overshoot so maybe leave as is - else: - self.scan_driving_phase = "behind" - - if self.scan_driving_phase == "behind": - if abs(self.rover_angle) > 0.12 and self.set_yaw is None and self.sim_time - self.scan_driving_phase_start < timedelta(seconds=50): - if (self.debug): - print(self.sim_time, self.robot_name, "Arrival handling: moving sideways") - self.publish("desired_movement", [GO_STRAIGHT, -9000, -math.copysign(0.5 * self.default_effort_level, self.rover_angle)]) - #TODO: keep doing until angle=0 - else: - self.scan_driving_phase = "waiting" - - if self.scan_driving_phase == "waiting": - if (self.debug): - print(self.sim_time, self.robot_name, "Arrival handling: final wait to straighten wheels") - if self.aligned_at is None: - self.aligned_at = self.sim_time - - if self.sim_time - self.aligned_at > timedelta(milliseconds=2500) or self.set_yaw is not None: - if self.driving_mode == "approach": - self.target_excavator_distance = EXCAVATOR_DIGGING_GAP - self.excavator_yaw = None - self.scan_driving = False - self.aligned_at = None - self.scan_driving_phase = "yaw" - self.scan_driving_phase_start = None - else: - self.publish("desired_movement", [0, 0, 0]) # going straight doesn't wait for steering so we have to wait here - -# else: -# if (self.debug): -# print(self.sim_time, self.robot_name, "Arrival handling: going forward") -# self.publish("desired_movement", [GO_STRAIGHT, 0, self.default_effort_level]) - - - -# self.rover_angle = rover_center_angle(data) - -# vim: expandtab sw=4 ts=4 diff --git a/moon/controller_round1.py b/moon/controller_round1.py deleted file mode 100644 index 7926f8e05..000000000 --- a/moon/controller_round1.py +++ /dev/null @@ -1,493 +0,0 @@ -""" - Space Robotics Challenge 2 -""" -import math -import heapq -import numpy as np - -from datetime import timedelta -from functools import partial -from shapely.geometry import Point, Polygon -from collections import defaultdict - -from osgar.bus import BusShutdownException -from osgar.lib import quaternion -from osgar.lib.quaternion import euler_zyx - -from osgar.lib.virtual_bumper import VirtualBumper - -from moon.controller import (pol2cart, ps, distance, SpaceRoboticsChallenge, VirtualBumperException, ChangeDriverException, VSLAMLostException, VSLAMFoundException, - VSLAMEnabledException, VSLAMDisabledException, LidarCollisionException, LidarCollisionMonitor) - -class SpaceRoboticsChallengeRound1(SpaceRoboticsChallenge): - def __init__(self, config, bus): - super().__init__(config, bus) - self.use_gimbal = False - - self.default_effort_level = 1000 # out of 1000; indicates how much effort compared to max should be used on driving - - self.volatile_queue = [] # each element is an array [index, name x, y] - self.volatile_last_submit_time = None - self.processing_plant_found = False - self.vslam_reset_at = None - self.vslam_valid = False - self.returning_to_base = False - self.vslam_fail_start = None - self.vslam_success_start = None - self.vslam_is_enabled = False - self.last_processing_plant_follow = None - - def get_extra_status(self): - return ps("Volatile queue length: %d" % len(self.volatile_queue)) - - def on_vslam_enabled(self, data): - super().on_vslam_enabled(data) - if self.vslam_is_enabled != data: - self.vslam_is_enabled = data - - if not self.true_pose or self.sim_time is None or self.last_position is None or self.yaw is None: - return - - if self.vslam_is_enabled: - raise VSLAMEnabledException() - else: - raise VSLAMDisabledException() - - def on_vslam_pose(self, data): - super().on_vslam_pose(data) - - if self.sim_time is None or self.last_position is None or self.yaw is None: - return - - if math.isnan(data[0][0]): # VSLAM not tracking - self.vslam_valid = False - if ( - self.tf['vslam']['trans_matrix'] is not None and - not self.inException and - not self.returning_to_base - ): - if self.vslam_fail_start is None: - self.vslam_fail_start = self.sim_time - elif self.sim_time - self.vslam_fail_start > timedelta(milliseconds=300): - self.vslam_fail_start = None - raise VSLAMLostException() - return - - self.vslam_fail_start = None - - if not math.isnan(data[0][0]): - self.vslam_valid = True - if self.vslam_success_start is None: - self.vslam_success_start = self.sim_time - if self.sim_time - self.vslam_success_start > timedelta(milliseconds=300): - self.vslam_success_start = None - if not self.inException and self.returning_to_base: - raise VSLAMFoundException() - - if self.vslam_reset_at is not None and self.processing_plant_found and self.sim_time - self.vslam_reset_at > timedelta(seconds=3) and not math.isnan(data[0][0]) and self.tf['vslam']['trans_matrix'] is None: - # request origin and start tracking in correct coordinates as soon as first mapping lock occurs - # TODO: another pose may arrive while this request is still being processed (not a big deal, just a ROS error message) - self.vslam_reset_at = None - self.send_request('request_origin', self.register_origin) - - def process_volatile_response(self, response, vol_index): - print(self.sim_time, "app: Response for obj %d, response: %s, guesses in queue: %d" % (vol_index, response, len(self.volatile_queue))) - if response == 'ok': - # if ok, no need to attempt the remaining guesses - i = 0 - while i < len(self.volatile_queue): - p, vol = self.volatile_queue[i] - _, obj_idx, _, _, _ = vol - if obj_idx == vol_index: - self.volatile_queue[i], self.volatile_queue[-1] = self.volatile_queue[-1], self.volatile_queue[i] - self.volatile_queue.pop() - else: - i += 1 - heapq.heapify(self.volatile_queue) - else: - # do nothing, ie keep going around and try to match the view - pass - - def on_sim_clock(self, t): - super().on_sim_clock(t) - if ( - self.sim_time is not None and - len(self.volatile_queue) > 0 and - (self.volatile_last_submit_time is None or self.sim_time - self.volatile_last_submit_time > timedelta(seconds=30)) - ): - priority, vol = heapq.heappop(self.volatile_queue) - guess_layer, obj_idx, obj_type, qx, qy = vol - self.volatile_last_submit_time = self.sim_time - print(self.sim_time, "app: Reporting obj %d, priority %.2f, guesses in queue: %d" % (obj_idx, priority, len(self.volatile_queue))) - self.send_request('artf %s %f %f 0.0' % (obj_type, qx, qy), partial(self.process_volatile_response, vol_index=obj_idx)) - - def on_artf(self, data): - artifact_type = data[0] - - # don't plan to handle anything but homebase - assert artifact_type == "homebase", artifact_type - - if self.sim_time is None or self.last_position is None or self.yaw is None: - return - - if not self.processing_plant_found: - self.processing_plant_found = True - raise ChangeDriverException() - - - def on_object_reached(self, data): - object_type, object_index = data - - if ( - self.tf['vslam']['trans_matrix'] is not None and - self.tf['vslam']['timestamp'] is not None and - self.sim_time - self.tf['vslam']['timestamp'] < timedelta(milliseconds=300) - ): - x,y,z = self.xyz - print(self.sim_time, "app: Object %s[%d] reached at [%.1f,%.1f]" % (object_type, object_index, x, y)) - - # covered_area = { idx: { layer: Polygon() } } - # https://www.researchgate.net/figure/Minimum-overlap-of-circles-covering-an-area_fig2_256607366 - # Minimum overlap of circles covering an area; r= 3.46 (=2* r * cos(30)); step 60 degrees, 6x - queued_count = 0 - covered_area = defaultdict(lambda: defaultdict(lambda: Polygon())) - for key, p in self.volatile_queue: - layer, idx, _, qx, qy = p - for l in range(layer, 3): - covered_area[idx][l] = covered_area[idx][l].union(Point(qx, qy).buffer(2)) - - olap = covered_area[object_index][0].intersection(Point(x,y).buffer(2)).area - if olap < 0.99 * 4 * math.pi: # if overlap is less than 100% - heapq.heappush(self.volatile_queue, (olap, [0, object_index, object_type, x, y])) - queued_count += 1 - - for i in range(6): - x_d,y_d = pol2cart(3.46, math.radians(i * 60)) - olap = covered_area[object_index][1].intersection(Point(x+x_d,y+y_d).buffer(2)).area - if olap < 0.99 * 4 * math.pi: - heapq.heappush(self.volatile_queue, (1000 + olap, [1, object_index, object_type, x+x_d, y+y_d])) - queued_count += 1 - - # further out non-overlapping guesses have higher priority than closer guesses with half circle or more of overlap with existing guesses - for i in range(12): - x_d,y_d = pol2cart(2*3.46 if i % 2 == 0 else 2*3, math.radians(i * 30)) - olap = covered_area[object_index][2].intersection(Point(x+x_d,y+y_d).buffer(2)).area - if olap < 0.99 * 4 * math.pi: - heapq.heappush(self.volatile_queue, (1006 + olap, [2, object_index, object_type, x+x_d, y+y_d])) - queued_count += 1 - - print(self.sim_time, "app: Queueing %d guesses" % (queued_count)) - if self.debug: - print("=== matplotlib start ===") - print("from matplotlib import pyplot as plt") - print("fig, ax = plt.subplots()") - print("ax.set_aspect(1)") - clrs = ['b', 'g', 'r', 'c', 'm', 'y', 'k'] - for i in range(len(self.volatile_queue)): - p, vol = self.volatile_queue[i] - _, idx, _, x, y = vol - print("ax.add_artist(plt.Circle((%.1f,%.1f),2,color='%s'))" % (x,y,clrs[idx % len(clrs)])) - print("plt.axis([-50, 50, -50, 50])") - print("plt.show()") - print("=== matplotlib end ===") - - def circular_pattern(self): - current_sweep_step = 0 - ARRIVAL_TOLERANCE = 5 - CIRCLE_SEGMENTS = 10 - sweep_steps = [] - #HOMEPOINT = self.xyz # where the drive started and we know the location well - CENTERPOINT = [3,-3] # where to start the circle from - HOMEPOINT = [19,5] # central area, good for regrouping - print(self.sim_time, "Home coordinates: %.1f, %.1f" % (HOMEPOINT[0], HOMEPOINT[1])) - - - #sweep_steps.append(["turn", [math.radians(360)]]) - sweep_steps.append(["goto", False, [HOMEPOINT, self.default_effort_level, False]]) - # sweep_steps.append(["turn", False, [math.radians(360)]]) # more likely to break vslam than help - - # go around the main crater in circles - for rad in range(13,55,2): # create a little overlap not to miss anything - for i in range(CIRCLE_SEGMENTS): - x,y = pol2cart(rad, 2 * math.pi - i * 2 * math.pi / CIRCLE_SEGMENTS) - sweep_steps.append(["goto", False, [[x + CENTERPOINT[0], y + CENTERPOINT[1]], self.default_effort_level, False]]) - - # sweep the hilly center in stripes (~10 mins), circle would include too many turns - # for i in range(-13, 13, 3): - # sweep_steps.append([[-15,i], -self.default_effort_level, False]) - # sweep_steps.append([[15,i], self.default_effort_level, False]) - - - #sweep_steps.append([[-7,-52], -self.default_effort_level, False]) # go backwards to starting point of right stripe - - # sweep right strip - #for i in range(-65, -40, 3): - # sweep_steps.append([[-55,i], -self.default_effort_level, False]) - # sweep_steps.append([[43,i], self.default_effort_level, False]) - - start_time = self.sim_time - wait_for_mapping = False - - self.virtual_bumper = VirtualBumper(timedelta(seconds=5), 0.2, angle_limit=math.pi/16) # radius of "stuck" area; a little more as the robot flexes - while current_sweep_step < len(sweep_steps) and self.sim_time - start_time < timedelta(minutes=60): - ex = None - try: - while wait_for_mapping: - self.wait(timedelta(seconds=1)) - - if not self.vslam_valid: - try: - with LidarCollisionMonitor(self, 1200): - try: - if self.last_processing_plant_follow is None or self.sim_time - self.last_processing_plant_follow > timedelta(seconds=60): - self.processing_plant_found = False - self.last_processing_plant_follow = self.sim_time - - self.turn(math.radians(self.rand.randrange(90,270)), timeout=timedelta(seconds=20)) - self.turn(math.radians(360), timeout=timedelta(seconds=40)) - except ChangeDriverException as e: - pass - finally: - self.processing_plant_found = True - self.go_straight(20.0, timeout=timedelta(seconds=40)) - except (VirtualBumperException, LidarCollisionException) as e: - self.inException = True - print(self.sim_time, self.robot_name, "Lidar or Virtual Bumper in random walk") - try: - self.go_straight(-3, timeout=timedelta(seconds=20)) - except: - pass - self.inException = False - continue - - with LidarCollisionMonitor(self, 1200): # some distance needed not to lose tracking when seeing only obstacle up front - - op, self.inException, params = sweep_steps[current_sweep_step] - if op == "goto": - pos, speed, self.returning_to_base = params - print(self.sim_time, "Driving radius: %.1f" % distance(CENTERPOINT, pos)) - self.go_to_location(pos, speed, full_turn=True, with_stop=False, tolerance=ARRIVAL_TOLERANCE, avoid_obstacles_close_to_destination=True) - elif op == "turn": - angle, self.returning_to_base = params - self.turn(angle, timeout=timedelta(seconds=20)) - elif op == "straight": # TODO: if interrupted, it will repeat the whole distance when recovered - dist, self.returning_to_base = params - self.go_straight(dist) - elif op == "rock": - self.drive_around_rock(6) # assume 6m the most needed - elif op == "lidar": - self.lidar_drive_around() - else: - assert False, "Invalid command" - current_sweep_step += 1 - except VSLAMLostException as e: - print("VSLAM lost") - self.returning_to_base = True - #sweep_steps.insert(current_sweep_step, ["goto", False, [HOMEPOINT, self.default_effort_level, True]]) - continue - - self.inException = True - - try: - self.go_straight(-3) # go 3m in opposite direction - except VSLAMDisabledException as e: - print(self.sim_time, "VSLAM: mapping disabled, waiting") - self.send_speed_cmd(0.0, 0.0) - wait_for_mapping = True - except VSLAMEnabledException as e: - print(self.sim_time, "VSLAM: mapping re-enabled") - wait_for_mapping = False - except VSLAMFoundException as e: - print("VSLAM found on way to base") - self.returning_to_base = False - current_sweep_step += 1 - except BusShutdownException: - raise - except: - pass - - self.inException = False - if not self.vslam_valid: # if vslam still not valid after backtracking, go towards center - self.returning_to_base = True - # sweep_steps.insert(current_sweep_step, ["goto", [HOMEPOINT, self.default_effort_level, True]]) - anglediff = self.get_angle_diff(HOMEPOINT) - # queue in reverse order - sweep_steps.insert(current_sweep_step, ["straight", False, [distance(self.xyz, HOMEPOINT), True]]) - sweep_steps.insert(current_sweep_step, ["turn", False, [anglediff, True]]) - #TODO: to go to homebase, measure angle and distance and then go straight instead of following localization, ignore all vslam info - else: - print("VSLAM found after stepping back") - except VSLAMFoundException as e: - print("VSLAM found on way to base") - self.returning_to_base = False - current_sweep_step += 1 - except VSLAMEnabledException as e: - print(self.sim_time, "VSLAM: mapping re-enabled") - # rover often loses precision after pausing, go back to center to re-localize - # sweep_steps.insert(current_sweep_step, ["goto", [HOMEPOINT, self.default_effort_level, False]]) # can't go every time - wait_for_mapping = False - except VSLAMDisabledException as e: - print(self.sim_time, "VSLAM: mapping disabled, waiting") - self.send_speed_cmd(0.0, 0.0) - wait_for_mapping = True - except LidarCollisionException as e: #TODO: long follow of obstacle causes loss, go along under steeper angle - print(self.sim_time, "Lidar") - if distance(self.xyz, pos) < ARRIVAL_TOLERANCE: - current_sweep_step += 1 - continue - sweep_steps.insert(current_sweep_step, ["lidar", True, []]) - except VirtualBumperException as e: - print(self.sim_time, "Bumper") - if distance(self.xyz, pos) < ARRIVAL_TOLERANCE: - current_sweep_step += 1 - continue - sweep_steps.insert(current_sweep_step, ["rock", True, []]) - sweep_steps.insert(current_sweep_step, ["straight", True, [-1, False]]) - - def stripe_to_obstacle_pattern(self): - # sweep the hilly center in stripes (~10 mins), circle would include too many turns - sweep_steps = [] - current_sweep_step = 0 - - sweep_steps.append([[-23,-42], "goto", -self.default_effort_level]) # TODO: go to first point more aggressively, do not skip to next step - for i in range(-42, 36, 3): - sweep_steps.append([[35, i], "goto", self.default_effort_level]) - sweep_steps.append([[-23, i], "goto", -self.default_effort_level]) - sweep_steps.append([[-23, i+3], "side", self.default_effort_level]) - - start_time = self.sim_time - - last_exception = self.sim_time - wait_for_mapping = False - while current_sweep_step < len(sweep_steps) and self.sim_time - start_time < timedelta(minutes=60): - ex = None - angle_diff = 0 - speed = 0 - pos = None - try: - while wait_for_mapping: - self.wait(timedelta(seconds=1)) - - self.virtual_bumper = VirtualBumper(timedelta(seconds=3), 0.2) # radius of "stuck" area; a little more as the robot flexes - with LidarCollisionMonitor(self, 1500): # some distance needed not to lose tracking when seeing only obstacle up front - pos, op, speed = sweep_steps[current_sweep_step] - if op == "goto": - angle_diff = self.get_angle_diff(pos,speed) - if current_sweep_step > 0 and abs(angle_diff) > math.radians(90): # past target, move to next - print(self.sim_time, "Target angle diff is more than 90deg..., already past the target, go to next step") - current_sweep_step += 1 - continue - if abs(angle_diff) > math.radians(10): # only turn if difference more than 10deg - ex = "turn" - self.turn(angle_diff, timeout=timedelta(seconds=15)) - ex = "goto" - self.go_to_location(pos, speed) - elif op == "side": - self.move_sideways(3, view_direction=0) # look towards 0deg - current_sweep_step += 1 - except VSLAMEnabledException as e: - print(self.sim_time, "VSLAM: mapping re-enabled") - wait_for_mapping = False - except VSLAMDisabledException as e: - print(self.sim_time, "VSLAM: mapping disabled, waiting") - self.send_speed_cmd(0.0, 0.0) - wait_for_mapping = True - except VSLAMLostException as e: - print(self.sim_time, "VSLAM lost") - self.inException = True - if ex == "turn": - self.turn(-angle_diff, timeout=timedelta(seconds=15)) - else: - self.go_straight(math.copysign(3, -speed)) # go 3m in opposite direction - self.inException = False - self.returning_to_base = True # continuing plan based on odometry, VSLAM will hopefully catch up - if not self.vslam_valid: - current_sweep_step += 1 - except VSLAMFoundException as e: - self.returning_to_base = False - print(self.sim_time, "VSLAM found") - except LidarCollisionException as e: - print(self.sim_time, "Lidar: stepping back from the obstacle...") - self.inException = True - self.go_straight(-2) - self.inException = False - if speed > 0: # if bump going forward, go backward to next leg - print(self.sim_time, "...and switching to backwards towards next leg") - current_sweep_step += 1 - else: - print(self.sim_time, "...and continuing backwards to existing target") - - except VirtualBumperException as e: - if distance(self.xyz, pos) < 5: - print(self.sim_time, "Bumper: Close enough, continuing to next leg") - current_sweep_step += 1 - continue - print(self.sim_time, "Bumper: trying to reverse") - self.inException = True - self.go_straight(math.copysign(2, -speed)) - self.inException = False - if self.sim_time - last_exception > timedelta(seconds=5): - last_exception = self.sim_time - print(self.sim_time, "- timeout, giving up on this leg, moving to next") - current_sweep_step += 1 - else: - print(self.sim_time, "- will try the same leg again") - - - - - def run(self): - - # chord length=2*sqrt(h * (2* radius - h)) where h is the distance from the circle boundary - # https://mathworld.wolfram.com/CircularSegment.html - - def set_homebase_found(response): - self.vslam_reset_at = self.sim_time - - try: - self.wait_for_init() - - self.set_cam_angle(0.1) - self.set_light_intensity("0.4") - self.set_brakes(False) - - try: - self.turn(math.radians(360), timeout=timedelta(seconds=20)) - except ChangeDriverException as e: - pass - except BusShutdownException: - raise - except: - pass - - self.processing_plant_found = True - self.send_speed_cmd(0.0, 0.0) - self.set_cam_angle(-0.1) - self.wait(timedelta(milliseconds=500)) - self.send_request('vslam_reset', set_homebase_found) - - while not self.true_pose: - self.update() - self.wait(timedelta(seconds=2)) # wait to receive first poses after true_pose before acting upon the current location - - # go to the nearest point on the circle? - - - #self.stripe_to_obstacle_pattern() - self.circular_pattern() - - except BusShutdownException: - pass - - print(self.sim_time, "FINISHED") - -def main(): - import argparse - - parser = argparse.ArgumentParser(description='Space Robotics Challenge 2') - args = parser.parse_args() - -if __name__ == "__main__": - main() - -# vim: expandtab sw=4 ts=4 diff --git a/moon/controller_round3.py b/moon/controller_round3.py deleted file mode 100644 index 8e59d2655..000000000 --- a/moon/controller_round3.py +++ /dev/null @@ -1,731 +0,0 @@ -""" - Space Robotics Challenge 2 -""" -import math -from statistics import median -from datetime import timedelta - -import numpy as np - -import moon.controller -from osgar.bus import BusShutdownException - -from moon.controller import pol2cart, cart2pol, best_fit_circle, SpaceRoboticsChallenge, ChangeDriverException, VirtualBumperException, LidarCollisionException, LidarCollisionMonitor -from osgar.lib.virtual_bumper import VirtualBumper -from moon.moonnode import CAMERA_WIDTH, CAMERA_HEIGHT, CAMERA_FOCAL_LENGTH, CAMERA_BASELINE, LIDAR_BEAM_SPACING - -CAMERA_ANGLE_DRIVING = 0.1 -CAMERA_ANGLE_LOOKING = 0.5 -CAMERA_ANGLE_HOMEBASE = 0.25 # look up while circling around homebase to avoid fake reflections from surrounding terrain -CUBESAT_MIN_EDGE_DISTANCE = 130 - -MAX_NR_OF_FARTHER_SCANS = 20 -HOMEBASE_KEEP_DISTANCE = 3 # maintain this distance from home base while approaching and going around -HOMEBASE_RADIUS = 4 # radius of the homebase structure (i.e, estimated 8m diameter) - -MAX_BASEMARKER_DISTANCE_HISTORY = 20 # do not act on a single lidar measurement, look at this many at a time and pick the min value -MAX_BASEMARKER_DISTANCE = 15 - -TURN_ON = 8 # radius of circle when turning -GO_STRAIGHT = float("inf") - -# DEBUG launch options -SKIP_CUBESAT_SUCCESS = False # skip cubesat, try to reach homebase directly -SKIP_HOMEBASE_SUCCESS = False # do not require successful homebase arrival report in order to look for alignment - -ATTEMPT_DELAY = timedelta(seconds=30) - - -def min_dist(laser_data): - if len(laser_data) > 0: - # remove ultra near reflections and unlimited values == 0 - laser_data = [x if x > 10 else 20000 for x in laser_data] # NASA scanner goes up to 15m of valid measurement - return min(laser_data)/1000.0 - return 0 - -def median_dist(laser_data): - if len(laser_data) > 0: - # remove ultra near reflections and unlimited values == 0 - laser_data = [x if x > 10 else 20000 for x in laser_data] # NASA scanner goes up to 15m of valid measurement - return median(laser_data)/1000.0 - return 0 - - -class SpaceRoboticsChallengeRound3(SpaceRoboticsChallenge): - def __init__(self, config, bus): - super().__init__(config, bus) - bus.register("desired_movement") - - self.default_effort_level = 800 - - self.cubesat_location = None - self.homebase_arrival_success = False - - self.cubesat_reached = False - self.cubesat_success = False - - self.basemarker_angle = None - self.basemarker_left_history = [] - self.basemarker_right_history = [] - self.basemarker_whole_scan_history = [] - self.basemarker_radius = None - self.centering = None - self.going_around_count = 0 - self.full_360_scan = False - self.full_360_objects = {} - - self.last_attempt_timestamp = None - - self.currently_following_object = { - 'object_type': None, - 'timestamp': None - } - - self.object_timeouts = { - 'homebase': timedelta(seconds=5), # tracking homebase based on visual - 'cubesat': timedelta(seconds=5), - 'basemarker': timedelta(milliseconds=200) - } - - - self.last_artefact_time = None - self.last_tracked_artefact = None - - self.objects_to_follow = [] - self.objects_in_view = {} - - - def follow_object(self, data): - self.objects_to_follow = data - self.last_attempt_timestamp = None - print (self.sim_time, "Starting to look for " + ','.join(data)) - - def on_driving_control(self, data): - super().on_driving_control(data) - - if data is None: - self.set_cam_angle(CAMERA_ANGLE_DRIVING) - print("Driving returned to main") - else: - print("Current driver: %s" % self.current_driver) - if self.current_driver == "cubesat": - self.set_cam_angle(CAMERA_ANGLE_LOOKING) - elif self.current_driver == "homebase": - self.set_cam_angle(CAMERA_ANGLE_DRIVING) - else: # basemarker - self.set_cam_angle(CAMERA_ANGLE_HOMEBASE) - if not self.inException: # do not interrupt driving if processing an exception - raise ChangeDriverException(data) - - def object_reached(self, object_type): - self.currently_following_object['object_type'] = None - self.currently_following_object['timestamp'] = None - - self.objects_to_follow.remove(object_type) - - if object_type == "homebase": # upon handover, robot should be moving straight - if self.cubesat_success: - if not self.homebase_arrival_success: - - def process_arrival(response): - print(self.sim_time, "app: Homebase response: %s" % response) - - if response == 'ok' or SKIP_HOMEBASE_SUCCESS: - self.set_cam_angle(CAMERA_ANGLE_HOMEBASE) - self.current_driver = "basemarker" - self.homebase_arrival_success = True - self.follow_object(['basemarker']) - - else: - # homebase arrival not accepted, keep trying after a delay - self.follow_object(['homebase']) - self.last_attempt_timestamp = self.sim_time - self.on_driving_control(None) # do this last as it raises exception - - self.send_request('artf homebase', process_arrival) - - else: - # homebase found (again), does not need reporting, just start basemarker search - self.set_cam_angle(CAMERA_ANGLE_HOMEBASE) - self.current_driver = "basemarker" - self.going_around_count += 1 - if self.debug: - print(self.sim_time, "Reached homebase again, setting circulation direction to %s" % ("left" if self.going_around_count % 2 == 0 else "right")) - self.follow_object(['basemarker']) - - else: - print(self.sim_time, "app: Reached reportable home base destination, need to find cubesat first though") - self.on_driving_control(None) # do this last as it raises exception - - elif object_type == 'basemarker': - print (self.sim_time, "app: Reporting alignment to server") - - def process_alignment(response): - print(self.sim_time, "app: Alignment response: %s" % response) - if response == 'ok': - # all done, exiting - exit - else: - self.going_around_count += 1 - self.follow_object(['basemarker']) - - self.send_request('artf homebase_alignment', process_alignment) - - - def interpolate_distance(self, pixels): - # linearly interpolate in between measured values (pixels, distance) - # line from 2 points: https://www.desmos.com/calculator/md6buy4efz - # plot 2D points: https://www.desmos.com/calculator/mhq4hsncnh - # plot 3D points: https://technology.cpm.org/general/3dgraph/ - - observed_values = [(294, 33.5), (370, 28.9), (396, 27.9), (499, 25.4), (565, 26), (589, 25.8), (661, 25.4), (958, 20), (1258, 17.5), (1635, 15.35), (2103, 13.56), (4594, 9.7)] - - t1 = None - - if pixels < observed_values[0][0]: - x2 = observed_values[1][0] - y2 = observed_values[1][1] - x1 = observed_values[0][0] - y1 = observed_values[0][1] - m = (y2 - y1) / (x2 - x1) - return m * (pixels - x1) + y1 - - else: - for t2 in observed_values: - if pixels < t2[0]: - x2 = t2[0] - y2 = t2[1] - x1 = t1[0] - y1 = t1[1] - m = (y2 - y1) / (x2 - x1) - return m * (pixels - x1) + y1 - else: - t1 = t2 - - i = len(observed_values) - 2 - x2 = observed_values[i+1][0] - y2 = observed_values[i+1][1] - x1 = observed_values[i][0] - y1 = observed_values[i][1] - m = (y2 - y1) / (x2 - x1) - return m * (pixels - x1) + y1 - - # used to follow objects (cubesat, processing plant, other robots, etc) - def on_artf(self, data): - # vol_type, x, y, w, h - # coordinates are pixels of bounding box - artifact_type = data[0] - - if self.sim_time is None: - return - - self.objects_in_view[artifact_type] = { - "expiration": self.sim_time + timedelta(milliseconds=200) - } - - if self.last_attempt_timestamp is not None and self.sim_time - self.last_attempt_timestamp < ATTEMPT_DELAY: - return - - - center_x = data[1] + data[3] / 2 - center_y = data[2] + data[4] / 2 - bbox_size = (data[3] + data[4]) / 2 # calculate avegage in case of substantially non square matches - img_x, img_y, img_w, img_h = data[1:5] - nr_of_nonblack = data[5] - - if self.full_360_scan: - if artifact_type not in self.full_360_objects.keys(): - self.full_360_objects[artifact_type] = [] - self.full_360_objects[artifact_type].append(self.yaw) - return - -# print ("Artf: %s %d %d %d %d %d" % (artifact_type, img_x, img_y, img_w, img_h, nr_of_nonblack)) - - # TODO if detection during turning on the spot, instead of driving straight steering a little, turn back to the direction where the detection happened first - - if ( - not self.in_driving_recovery and - self.objects_to_follow and - artifact_type in self.objects_to_follow - ): # if in exception, let the exception handling take its course - if self.currently_following_object['object_type'] is None: - self.currently_following_object['object_type'] = artifact_type - self.currently_following_object['timestamp'] = self.sim_time - print (self.sim_time, "Starting to track %s" % artifact_type) - self.on_driving_control(artifact_type) - else: - for looking_for in self.objects_to_follow: - if self.currently_following_object['object_type'] == looking_for: # detected artefact is top priority and it is the one being followed already, continue what you were doing - break - elif looking_for == artifact_type: # we are looking for this artifact but it's not the one currently being followed, switch - print (self.sim_time, "Switching to tracking %s" % artifact_type) - self.currently_following_object['object_type'] = artifact_type - self.on_driving_control(artifact_type) - - if self.currently_following_object['object_type'] == artifact_type: - self.currently_following_object['timestamp'] = self.sim_time - - if self.currently_following_object['object_type'] == 'cubesat': - # print("Cubesat reported at %d %d %d %d" % (data[1], data[2], data[3], data[4])) - # virtual bumper still applies while this block has control. When triggered, driving will go to recovery and main will take over driving - - # when cubesat disappears, we need to reset the steering to going straight - # NOTE: light does not shine in corners of viewport, need to report sooner or turn first - # box is big enough and close to the top edge and camera is pointing up and far enough from x edges, report - if ( - bbox_size > 25 and - img_y < 40 and - abs(self.sensor_joint_position - (CAMERA_ANGLE_LOOKING + self.pitch)) < 0.1 and # camera is close to 'looking up' position - CUBESAT_MIN_EDGE_DISTANCE < img_x < (CAMERA_WIDTH - CUBESAT_MIN_EDGE_DISTANCE - bbox_size) - ): - # box 25 pixels represents distance about 27m which is as close as we can possibly get for cubesats with high altitude - # object in center (x axis) and close enough (bbox size) - # stop and report angle and distance from robot - # robot moves a little after detection so the angles do not correspond with the true pose we will receive - # TODO: if found during side sweep, robot will turn some between last frame and true pose messing up the angle - self.publish("desired_movement", [0, 0, 0]) - print(self.sim_time, "app: cubesat final frame x=%d y=%d w=%d h=%d" % (data[1], data[2], data[3], data[4])) - - if 'homebase' in self.objects_to_follow: - self.objects_to_follow.remove('homebase') # do not immediately follow homebase if it was secondary to give main a chance to report cubesat - - def process_origin(message): - self.register_origin(message) - nonlocal data - if message.split()[0] == 'origin': - origin = [float(x) for x in message.split()[1:]] - self.xyz = origin[:3] - qx, qy, qz, qw = origin[3:] - - print(self.sim_time, "Origin received, internal position updated") - # robot should be stopped right now (using brakes once available) - # lift camera to max, object should be (back) in view - # trigger recognition, get bounding box and calculate fresh angles - - # TODO: this separate storage of reported numbers is temporary, need OSGAR to accept true values and update its own data - self.nasa_xyz = self.xyz - sinr_cosp = 2 * (qw * qx + qy * qz); - cosr_cosp = 1 - 2 * (qx * qx + qy * qy); - self.nasa_roll = math.atan2(sinr_cosp, cosr_cosp); - - sinp = 2 * (qw * qy - qz * qx); - if abs(sinp) >= 1: - self.nasa_pitch = math.copysign(math.pi / 2, sinp); - else: - self.nasa_pitch = math.asin(sinp); - - self.nasa_pitch = - self.nasa_pitch # for subsequent calculations, up is positive and down is negative - - siny_cosp = 2 * (qw * qz + qx * qy); - cosy_cosp = 1 - 2 * (qy * qy + qz * qz); - self.nasa_yaw = math.atan2(siny_cosp, cosy_cosp); - print (self.sim_time, "app: True pose received: xyz=[%f,%f,%f], roll=%f, pitch=%f, yaw=%f" % (origin[0],origin[1],origin[2],self.nasa_roll, self.nasa_pitch, self.nasa_yaw)) - - print(self.sim_time, "app: Final frame x=%d y=%d w=%d h=%d, nonblack=%d" % (data[1], data[2], data[3], data[4], data[5])) - screen_x = (CAMERA_WIDTH / 2 - (img_x + img_w/2) ) - screen_y = (CAMERA_HEIGHT / 2 - (img_y + img_h/2) ) - - (rho, phi) = cart2pol(screen_x, screen_y) - phi += self.nasa_roll - (screen_x, screen_y) = pol2cart(rho, phi) - - angle_x = math.atan( screen_x / float(CAMERA_FOCAL_LENGTH)) - angle_y = math.atan( screen_y / float(CAMERA_FOCAL_LENGTH)) - - distance = self.interpolate_distance(nr_of_nonblack) - ax = self.nasa_yaw + angle_x - ay = self.nasa_pitch + angle_y - - print (self.sim_time, "app: Camera angle adjustment: %f" % self.sensor_joint_position) - ay += self.sensor_joint_position - - x, y, z = self.nasa_xyz - print("Using pose: xyz=[%f %f %f] orientation=[%f %f %f]" % (x, y, z, self.nasa_roll, self.nasa_pitch, self.nasa_yaw)) - print("In combination with view angle %f %f and distance %f" % (ax, ay, distance)) - ox = math.cos(ax) * math.cos(ay) * distance - oy = math.sin(ax) * math.cos(ay) * distance - oz = math.sin(ay) * distance - self.cubesat_location = (x+ox, y+oy, z+oz) - print (self.sim_time, "app: Object offset calculated at: [%f %f %f]" % (ox, oy, oz)) - print (self.sim_time, "app: Reporting estimated object location at: [%f,%f,%f]" % (x+ox, y+oy, z+oz)) - - def process_apriori_object(response): - if response == 'ok': - print("app: Apriori object reported correctly") - self.cubesat_success = True - self.object_reached("cubesat") - self.follow_object(['homebase']) - else: - print("app: Estimated object location incorrect, wait before continuing task; response: %s" % str(response)) - self.last_attempt_timestamp = self.sim_time - - s = '%s %.2f %.2f %.2f\n' % (artifact_type, x+ox, y+oy, z+oz) - self.send_request('artf ' + s, process_apriori_object) - else: - print(self.sim_time, "Origin request failed") # TODO: in future, we should try to find the cubesat again based on accurate position tracking - self.last_attempt_timestamp = self.sim_time - - # TODO: object was reached but not necessarily successfully reported; - # for now, just return driving to main which will either drive randomly with no additional purpose if homebase was previously found or will look for homebase - self.current_driver = None - self.on_driving_control(None) # do this last as it raises exception - - self.send_request('request_origin', process_origin) - - elif center_x < 200: # if cubesat near left edge, turn left - if img_y > 20: # if far enough from top, go straight too, otherwise turn in place - self.publish("desired_movement", [TURN_ON, 0, self.default_effort_level]) - else: - self.publish("desired_movement", [0, 0, self.default_effort_level]) - elif center_x > 440: - if img_y > 20: # if far enough from top, go straight too, otherwise turn in place - self.publish("desired_movement", [-TURN_ON, 0, self.default_effort_level]) - else: - self.publish("desired_movement", [0, 0, -self.default_effort_level]) - else: - # bbox is ahead but too small or position not near the edge, continue straight - self.publish("desired_movement", [GO_STRAIGHT, 0, self.default_effort_level]) - - - elif self.currently_following_object['object_type'] == 'homebase': - if center_x < (CAMERA_WIDTH/2 - 20): # if homebase to the left, steer left - self.publish("desired_movement", [TURN_ON, 0, self.default_effort_level]) - elif center_x > (CAMERA_WIDTH/2 + 20): - self.publish("desired_movement", [-TURN_ON, 0, self.default_effort_level]) - else: # if centered, keep going straight - self.publish("desired_movement", [GO_STRAIGHT, 0, self.default_effort_level]) - - elif self.currently_following_object['object_type'] == 'basemarker': - self.basemarker_angle = math.atan( (CAMERA_WIDTH / 2 - center_x ) / float(CAMERA_FOCAL_LENGTH)) - - - - def on_scan(self, data): - assert len(data) == 180 - super().on_scan(data) - - delete_in_view = [artf for artf in self.objects_in_view if self.objects_in_view[artf]['expiration'] < self.sim_time] - for artf in delete_in_view: - del self.objects_in_view[artf] - - if self.last_attempt_timestamp is not None and self.sim_time - self.last_attempt_timestamp < ATTEMPT_DELAY: - return - - # if was following an artefact but it disappeared, just go straight until another driver takes over - if ( - self.currently_following_object['timestamp'] is not None and - self.currently_following_object['object_type'] is not None and - self.sim_time - self.currently_following_object['timestamp'] > self.object_timeouts[self.currently_following_object['object_type']] - - ): - self.publish("desired_movement", [GO_STRAIGHT, 0, self.default_effort_level]) - print (self.sim_time, "No longer tracking %s" % self.currently_following_object['object_type']) - self.currently_following_object['timestamp'] = None - self.currently_following_object['object_type'] = None - self.basemarker_angle = None - if self.current_driver != "basemarker": # do not change drivers when basemarker gets out of view because going around will bring it again - self.on_driving_control(None) # do this last as it raises exception - - - # NASA sends 100 samples over 2.6rad (~150 degrees) (each increment is 0.0262626260519rad (~15 degrees)) - # OSGAR pads in in front and in back to 180 samples, first and last 40 are zeros - -# TODO: if too far from anything, revert to looking for homebase - if not self.in_driving_recovery: - - midindex = len(data) // 2 - # 10 degrees left and right is 6-7 samples before and after the array midpoint - straight_ahead_dist = min_dist(data[midindex-15:midindex+15]) - - if self.currently_following_object['object_type'] == 'homebase': -# print (self.sim_time, "controller_round3: homebase current distance: %f" % (straight_ahead_dist)) - - if straight_ahead_dist < HOMEBASE_KEEP_DISTANCE: - self.publish("desired_movement", [0, 0, 0]) - - print ("app: final homebase distance %f: " % straight_ahead_dist) - self.object_reached('homebase') - - if 'basemarker' in self.objects_to_follow: - - if 'homebase' not in self.objects_in_view.keys(): - # lost contact with homebase, try approach again - print (self.sim_time, "app: No longer going around homebase") - self.centering = None - self.currently_following_object['timestamp'] = None - self.currently_following_object['object_type'] = None - self.basemarker_angle = None - self.basemarker_right_history = self.basemarker_left_history = [] - self.follow_object(['homebase']) - self.on_driving_control(None) # do this last as it possibly raises exception - return - - # find min_index where distance < 5 and max_index where distance < 5 - # find circle passing through these points (defined in polar coordinates as [index*6 degrees, distance] - min_index = max_index = None - for i in range(0,180): - if 10 < data[i] < 5000: - min_index = i - break - for i in range(180, 0, -1): - if 10 < data[i-1] < 5000: - max_index = i - break - - if min_index is None or max_index is None or max_index - min_index < 3: - # if in basemarker mode, looking at homebase but lidar shows no hits, it's a noisy lidar scan, ignore - if self.debug: - print(self.sim_time, "No lidar hits closer than 5m, abandoning circle calculations") - return - - x_l = [] - y_l = [] - for i in range(min_index, max_index): - x,y = pol2cart(data[i] / 1000.0, -1.29999995232 + (i - 40) * LIDAR_BEAM_SPACING) - x_l.append(x) - y_l.append(y) - - homebase_cx, homebase_cy, homebase_radius = best_fit_circle(x_l, y_l) - if self.debug: - print (self.sim_time, "Homebase center: [%f,%f], radius: %f" % (homebase_cx, homebase_cy, homebase_radius)) - - # since we are looking through left camera, cy will be calculated relative to the camera - # need it relative to the center of the robot - homebase_cy -= CAMERA_BASELINE / 2 - - right_dist = median_dist(data[midindex-8:midindex-6]) - left_dist = median_dist(data[midindex+6:midindex+8]) - self.basemarker_left_history.append(left_dist) - self.basemarker_right_history.append(right_dist) - self.basemarker_whole_scan_history.append(min_dist(data)) - if len(self.basemarker_right_history) > MAX_BASEMARKER_DISTANCE_HISTORY: - self.basemarker_right_history.pop(0) - if len(self.basemarker_left_history) > MAX_BASEMARKER_DISTANCE_HISTORY: - self.basemarker_left_history.pop(0) - if len(self.basemarker_whole_scan_history) > MAX_BASEMARKER_DISTANCE_HISTORY: - self.basemarker_whole_scan_history.pop(0) - left_dist = min(self.basemarker_left_history) - right_dist = min(self.basemarker_right_history) - - # print (self.sim_time, "app: Min dist front: %f, dist left=%f, right=%f" % (straight_ahead_dist, left_dist, right_dist)) - - if self.basemarker_angle is not None and abs(self.basemarker_angle) < 0.06 and left_dist < 6 and abs(homebase_cy) < 0.1: # cos 20 = dist_r / dist _l is the max ratio in order to be at most 10 degrees off; also needs to be closer than 6m - self.publish("desired_movement", [0, -9000, 0]) - self.object_reached('basemarker') - return - - # if re-centering rover towards the homebase, keep turning until very close to centered (as opposed to within the target range) - if self.centering is not None and abs(homebase_cy > 0.1): - self.publish("desired_movement", self.centering) - if self.debug: - print(self.sim_time, "Recentering: continue previous movement command") - return - else: - self.centering = None - - # if seeing basemarker and homebase center is not straight ahead OR if looking past homebase in one of the directions, turn in place to adjust - # -1 means going right, 1 going left - circle_direction = 1 if self.going_around_count % 2 == 0 else -1 - if (self.currently_following_object['object_type'] == 'basemarker' and homebase_cy < -0.2) or left_dist > 10: - self.centering = [0, -9000, -self.default_effort_level] - self.publish("desired_movement", self.centering) - elif (self.currently_following_object['object_type'] == 'basemarker' and homebase_cy > 0.2) or right_dist > 10: - self.centering = [0, -9000, self.default_effort_level] - self.publish("desired_movement", self.centering) - elif left_dist < 1.5 or right_dist < 1.5: - # if distance is closer than 1.5m, pull back - self.publish("desired_movement", [GO_STRAIGHT, 0, -self.default_effort_level]) - elif left_dist > HOMEBASE_KEEP_DISTANCE + 1 or right_dist > HOMEBASE_KEEP_DISTANCE + 1: - # if distance further than 1m more than optimal, come closer - self.publish("desired_movement", [GO_STRAIGHT, 0, self.default_effort_level]) - elif homebase_cy < -1: - self.centering = [0, -9000, -self.default_effort_level] - self.publish("desired_movement", self.centering) - elif homebase_cy > 1: - self.centering = [0, -9000, self.default_effort_level] - self.publish("desired_movement", self.centering) - else: - # print ("driving radius: %f" % self.basemarker_radius) - # negative radius turns to the right - if self.currently_following_object['object_type'] == 'basemarker' and self.basemarker_angle is not None: - (rho, phi) = cart2pol(homebase_cx, homebase_cy) - circle_direction = -1 if self.basemarker_angle < phi else 1 - self.publish("desired_movement", [-(HOMEBASE_KEEP_DISTANCE + HOMEBASE_RADIUS), -9000, circle_direction * self.default_effort_level]) - - - - def run(self): - - try: - self.wait_for_init() - self.set_light_intensity("1.0") - self.set_brakes(False) - # some random manual starting moves to choose from -# self.go_straight(-0.1, timeout=timedelta(seconds=20)) -# self.go_straight(-2, timeout=timedelta(seconds=20)) -# self.turn(math.radians(45), timeout=timedelta(seconds=20)) -# self.set_cam_angle(CAMERA_ANGLE_HOMEBASE) - - - - if SKIP_CUBESAT_SUCCESS: - # skip cubesat - self.cubesat_success = True - self.follow_object(['homebase']) - else: - # regular launch - self.follow_object(['cubesat', 'homebase']) - -# self.homebase_arrival_success = True -# self.cubesat_success = True -# self.follow_object(['homebase']) -# self.follow_object(['basemarker']) -# self.current_driver = 'basemarker' - - # self.publish("desired_movement", [10, 9000, 10]) -# self.wait(timedelta(seconds=10)) - - last_walk_start = 0.0 - start_time = self.sim_time - while self.sim_time - start_time < timedelta(minutes=45): - additional_turn = 0 - last_walk_start = self.sim_time - - # TURN 360 - # TODO: - # if looking for multiple objects, sweep multiple times, each looking only for one object (objects listed in order of priority) - # this way we won't start following homebase when cubesat was also visible, but later in the sweep - # alternatively, sweep once without immediately reacting to matches but note match angles - # then turn back to the direction of the top priority match - - if self.current_driver is None and not self.brakes_on: - if len(self.objects_to_follow) > 1: - self.full_360_scan = True - try: - self.virtual_bumper = VirtualBumper(timedelta(seconds=20), 0.1) - with LidarCollisionMonitor(self): - # start each straight stretch by looking around first - # if cubesat already found, we are looking for homebase, no need to lift camera as much - self.set_cam_angle(CAMERA_ANGLE_DRIVING if self.cubesat_success else CAMERA_ANGLE_LOOKING) - self.turn(math.radians(360), timeout=timedelta(seconds=20)) - except ChangeDriverException as e: - print(self.sim_time, "Turn interrupted by driver: %s" % e) - self.full_360_scan = False - self.full_360_objects = {} - continue - # proceed to straight line drive where we wait; straight line exception handling is better applicable for follow-object drive - except (VirtualBumperException, LidarCollisionException) as e: - self.inException = True - self.set_cam_angle(CAMERA_ANGLE_DRIVING) - print(self.sim_time, "Turn 360 degrees Virtual Bumper!") - self.virtual_bumper = None - # recovery from an exception while turning CCW is to turn CW somewhat - deg_angle = self.rand.randrange(-180, -90) - self.turn(math.radians(deg_angle), timeout=timedelta(seconds=10)) - self.inException = False - self.bus.publish('driving_recovery', False) - if len(self.objects_to_follow) > 1: - print(self.sim_time, "app: 360deg scan: " + str([[a, median(self.full_360_objects[a])] for a in self.full_360_objects.keys()])) - for o in self.objects_to_follow: - if o in self.full_360_objects.keys(): - try: - self.virtual_bumper = VirtualBumper(timedelta(seconds=20), 0.1) - with LidarCollisionMonitor(self): - self.turn(median(self.full_360_objects[o]) - self.yaw, timeout=timedelta(seconds=20)) - except BusShutdownException: - raise - except: - # in case it gets stuck turning, just hand over driving to main without completing the desired turn - pass - break - self.full_360_scan = False - self.full_360_objects = {} - - else: - try: - self.virtual_bumper = VirtualBumper(timedelta(seconds=20), 0.1) - with LidarCollisionMonitor(self): - self.wait(timedelta(minutes=2)) # allow for self driving, then timeout - except ChangeDriverException as e: - # if driver lost, start by turning 360; if driver changed, wait here again - continue - except (VirtualBumperException, LidarCollisionException) as e: - print(self.sim_time, "Follow-object Virtual Bumper") - self.inException = True - print(self.sim_time, repr(e)) - last_walk_end = self.sim_time - self.virtual_bumper = None - self.set_cam_angle(CAMERA_ANGLE_DRIVING) - self.go_straight(-2.0, timeout=timedelta(seconds=10)) # this should be reasonably safe, we just came from there - self.try_step_around() - self.inException = False - self.bus.publish('driving_recovery', False) - - # GO STRAIGHT - try: - self.virtual_bumper = VirtualBumper(timedelta(seconds=4), 0.1) - with LidarCollisionMonitor(self): - if self.current_driver is None and not self.brakes_on: - self.set_cam_angle(CAMERA_ANGLE_DRIVING) - self.go_straight(30.0, timeout=timedelta(minutes=2)) - else: - self.wait(timedelta(minutes=2)) # allow for self driving, then timeout - self.update() - except ChangeDriverException as e: - continue - except (VirtualBumperException, LidarCollisionException) as e: - print(self.sim_time, "Go Straight or Follow-object Virtual Bumper") - self.inException = True - # TODO: crashes if an exception (e.g., excess pitch) occurs while handling an exception (e.g., virtual/lidar bump) - print(self.sim_time, repr(e)) - last_walk_end = self.sim_time - self.virtual_bumper = None - self.set_cam_angle(CAMERA_ANGLE_DRIVING) - self.go_straight(-2.0, timeout=timedelta(seconds=10)) # this should be reasonably safe, we just came from there - if last_walk_end - last_walk_start > timedelta(seconds=20): # if we went more than 20 secs, try to continue a step to the left - # TODO: this is not necessarily safe, need to protect against pitch, etc again - self.try_step_around() - else: - self.bus.publish('driving_recovery', False) - - self.inException = False - - print ("Time elapsed since start of previous leg: %d sec" % (last_walk_end.total_seconds()-last_walk_start.total_seconds())) - if last_walk_end - last_walk_start > timedelta(seconds=40): - # if last step only ran short time before bumper (this includes bumper timeout time), time for a large random turn - # if it ran long time, maybe worth trying going in the same direction - continue - additional_turn = 30 - - # next random walk direction should be between 30 and 150 degrees - # (no need to go straight back or keep going forward) - # if part of virtual bumper handling, add 30 degrees to avoid the obstacle more forcefully - - - # TURN RANDOMLY - deg_angle = self.rand.randrange(30 + additional_turn, 150 + additional_turn) - deg_sign = self.rand.randint(0,1) - if deg_sign: - deg_angle = -deg_angle - try: - self.virtual_bumper = VirtualBumper(timedelta(seconds=20), 0.1) - with LidarCollisionMonitor(self): - self.turn(math.radians(deg_angle), timeout=timedelta(seconds=30)) - except ChangeDriverException as e: - continue - except (VirtualBumperException, LidarCollisionException): - self.inException = True - self.set_cam_angle(CAMERA_ANGLE_DRIVING) - print(self.sim_time, "Random Turn Virtual Bumper!") - self.virtual_bumper = None - if self.current_driver is not None: - # probably didn't throw in previous turn but during self driving - self.go_straight(-2.0, timeout=timedelta(seconds=10)) - self.try_step_around() - self.turn(math.radians(-deg_angle), timeout=timedelta(seconds=30)) - self.inException = False - self.bus.publish('driving_recovery', False) - except BusShutdownException: - pass - -# vim: expandtab sw=4 ts=4 diff --git a/moon/docker/build.bash b/moon/docker/build.bash deleted file mode 100755 index b75aa062e..000000000 --- a/moon/docker/build.bash +++ /dev/null @@ -1,63 +0,0 @@ -#!/usr/bin/env bash - -# -# Copyright (C) 2018 Open Source Robotics Foundation -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -# -# - -# Builds a Docker image. -while getopts h arg; do - case $arg in - h) - echo "Usage: $0 " - echo "Requires the following folders to be present in $HOME/space-challenge/" - echo "* srcp2-competitors from https://gitlab.com/scheducation/srcp2-competitors/" - echo "* osgar .. this repository" - exit 1 - ;; - esac -done - -DIR_ARG=${@:$OPTIND:1} - -# get path to current directory -DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" - -if [ ! -d $DIR/$DIR_ARG ] -then - echo "image-name must be a directory in the same folder as this script" - exit 2 -fi - -if [[ ! -d $HOME/space-challenge/srcp2-competitors || ! -d $HOME/space-challenge/osgar ]]; then - echo "Required folders missing from staging directory, run with -h switch for help" - exit 2 -fi - -user_id=$(id -u) -image_name=$(basename $DIR_ARG) -image_plus_tag=$image_name:$(date +%Y_%m_%d_%H%M) -# mercurial adds a + symbol if there are uncomitted changes in the repo -# that will break docker tag syntax -#hg_id=$(hg id -i | tr -d '+') - -shift - - -docker build --rm -t $image_plus_tag -f $DIR/$image_name/Dockerfile ${HOME}/space-challenge/ --build-arg NUM_THREADS=$(nproc) -docker tag $image_plus_tag $image_name:latest -#docker tag $image_plus_tag $image_name:$hg_id - -echo "Built $image_plus_tag and tagged as $image_name:latest" # and $image_name:$hg_id" diff --git a/moon/docker/rover/Dockerfile b/moon/docker/rover/Dockerfile deleted file mode 100644 index 2ca37b447..000000000 --- a/moon/docker/rover/Dockerfile +++ /dev/null @@ -1,260 +0,0 @@ -FROM ros:melodic - -ENV DEBIAN_FRONTEND noninteractive - -# install dependencies via apt -ENV DEBCONF_NOWARNINGS yes - -# install ros packages -RUN apt-get update && apt-get install -y \ - ros-melodic-catkin \ - ros-melodic-cv-bridge \ - ros-melodic-tf2-geometry-msgs \ - ros-melodic-image-transport \ - && rm -rf /var/lib/apt/lists/* - -RUN set -x && \ - apt-get update -y -qq && \ - apt-get upgrade -y -qq --no-install-recommends && \ - : "basic dependencies" && \ - apt-get install -y -qq \ - build-essential \ - pkg-config \ - cmake \ - git \ - wget \ - less \ - curl \ - vim \ - tar \ - unzip && \ - : "ROS dependencies" && \ - apt-get install -y -qq \ - python && \ - : "g2o dependencies" && \ - apt-get install -y -qq \ - libgoogle-glog-dev \ - libatlas-base-dev \ - libsuitesparse-dev \ - libglew-dev && \ - : "OpenCV dependencies" && \ - apt-get install -y -qq \ - libgtk-3-dev \ - libjpeg-dev \ - libpng++-dev \ - libtiff-dev \ - libopenexr-dev \ - libwebp-dev \ - ffmpeg \ - libavcodec-dev \ - libavformat-dev \ - libavutil-dev \ - libswscale-dev \ - libavresample-dev && \ - : "other dependencies" && \ - apt-get install -y -qq \ - libyaml-cpp-dev && \ - : "remove cache" && \ - apt-get autoremove -y -qq && \ - rm -rf /var/lib/apt/lists/* - -ARG CMAKE_INSTALL_PREFIX=/usr/local -ARG NUM_THREADS=1 - -ENV CPATH=${CMAKE_INSTALL_PREFIX}/include:${CPATH} -ENV C_INCLUDE_PATH=${CMAKE_INSTALL_PREFIX}/include:${C_INCLUDE_PATH} -ENV CPLUS_INCLUDE_PATH=${CMAKE_INSTALL_PREFIX}/include:${CPLUS_INCLUDE_PATH} -ENV LIBRARY_PATH=${CMAKE_INSTALL_PREFIX}/lib:${LIBRARY_PATH} -ENV LD_LIBRARY_PATH=${CMAKE_INSTALL_PREFIX}/lib:${LD_LIBRARY_PATH} - -# Eigen -ARG EIGEN3_VERSION=3.3.7 -WORKDIR /tmp -RUN set -x && \ - wget -q https://gitlab.com/libeigen/eigen/-/archive/${EIGEN3_VERSION}/eigen-${EIGEN3_VERSION}.tar.bz2 && \ - tar xf eigen-${EIGEN3_VERSION}.tar.bz2 && \ - rm -rf eigen-${EIGEN3_VERSION}.tar.bz2 && \ - cd eigen-${EIGEN3_VERSION} && \ - mkdir -p build && \ - cd build && \ - cmake \ - -DCMAKE_BUILD_TYPE=Release \ - -DCMAKE_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX} \ - .. && \ - make -j${NUM_THREADS} && \ - make install && \ - cd /tmp && \ - rm -rf * -ENV Eigen3_DIR=${CMAKE_INSTALL_PREFIX}/share/eigen3/cmake - -# g2o -ARG G2O_COMMIT=9b41a4ea5ade8e1250b9c1b279f3a9c098811b5a -WORKDIR /tmp -RUN set -x && \ - git clone https://github.com/RainerKuemmerle/g2o.git && \ - cd g2o && \ - git checkout ${G2O_COMMIT} && \ - mkdir -p build && \ - cd build && \ - cmake \ - -DCMAKE_BUILD_TYPE=Release \ - -DCMAKE_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX} \ - -DBUILD_SHARED_LIBS=ON \ - -DBUILD_UNITTESTS=OFF \ - -DBUILD_WITH_MARCH_NATIVE=OFF \ - -DG2O_USE_CHOLMOD=OFF \ - -DG2O_USE_CSPARSE=ON \ - -DG2O_USE_OPENGL=OFF \ - -DG2O_USE_OPENMP=ON \ - -DG2O_BUILD_APPS=OFF \ - -DG2O_BUILD_EXAMPLES=OFF \ - -DG2O_BUILD_LINKED_APPS=OFF \ - .. && \ - make -j${NUM_THREADS} && \ - make install && \ - cd /tmp && \ - rm -rf * -ENV g2o_DIR=${CMAKE_INSTALL_PREFIX}/lib/cmake/g2o - -# OpenCV -ARG OPENCV_VERSION=3.4.10 -WORKDIR /tmp -RUN set -x && \ - wget -q https://github.com/opencv/opencv/archive/${OPENCV_VERSION}.zip && \ - unzip -q ${OPENCV_VERSION}.zip && \ - rm -rf ${OPENCV_VERSION}.zip && \ - cd opencv-${OPENCV_VERSION} && \ - mkdir -p build && \ - cd build && \ - cmake \ - -DCMAKE_BUILD_TYPE=Release \ - -DCMAKE_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX} \ - -DBUILD_DOCS=OFF \ - -DBUILD_EXAMPLES=OFF \ - -DBUILD_JASPER=OFF \ - -DBUILD_OPENEXR=OFF \ - -DBUILD_PERF_TESTS=OFF \ - -DBUILD_TESTS=OFF \ - -DBUILD_opencv_apps=OFF \ - -DBUILD_opencv_dnn=OFF \ - -DBUILD_opencv_ml=OFF \ - -DBUILD_opencv_python_bindings_generator=OFF \ - -DENABLE_CXX11=ON \ - -DENABLE_FAST_MATH=ON \ - -DWITH_EIGEN=ON \ - -DWITH_FFMPEG=ON \ - -DWITH_OPENMP=ON \ - .. && \ - make -j${NUM_THREADS} && \ - make install && \ - cd /tmp && \ - rm -rf * -ENV OpenCV_DIR=${CMAKE_INSTALL_PREFIX}/lib/cmake/opencv - -# DBoW2 -ARG DBOW2_COMMIT=687fcb74dd13717c46add667e3fbfa9828a7019f -WORKDIR /tmp -RUN set -x && \ - git clone https://github.com/shinsumicco/DBoW2.git && \ - cd DBoW2 && \ - git checkout ${DBOW2_COMMIT} && \ - mkdir -p build && \ - cd build && \ - cmake \ - -DCMAKE_BUILD_TYPE=Release \ - -DCMAKE_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX} \ - .. && \ - make -j${NUM_THREADS} && \ - make install && \ - cd /tmp && \ - rm -rf * -ENV DBoW2_DIR=${CMAKE_INSTALL_PREFIX}/lib/cmake/DBoW2 - -# protobuf -WORKDIR /tmp -RUN set -x && \ - apt-get update -y -qq && \ - apt-get upgrade -y -qq --no-install-recommends && \ - apt-get install -y -qq autogen autoconf libtool && \ - wget -q https://github.com/google/protobuf/archive/v3.6.1.tar.gz && \ - tar xf v3.6.1.tar.gz && \ - cd protobuf-3.6.1 && \ - ./autogen.sh && \ - ./configure --prefix=${CMAKE_INSTALL_PREFIX} --enable-static=no && \ - make -j${NUM_THREADS} && \ - make install && \ - cd /tmp && \ - rm -rf * && \ - apt-get purge -y -qq autogen autoconf libtool && \ - apt-get autoremove -y -qq && \ - rm -rf /var/lib/apt/lists/* - -# OpenVSLAM -WORKDIR / -ARG OPENVSLAM_COMMIT=026827ebd59825de42463f853e910e9ccb44f0ef -RUN set -x && \ - git clone https://github.com/frantisekbrabec/openvslam.git && \ - cd openvslam && \ - git checkout ${OPENVSLAM_COMMIT} && \ - mkdir -p build && \ - cd build && \ - cmake \ - -DBUILD_WITH_MARCH_NATIVE=OFF \ - -DUSE_PANGOLIN_VIEWER=OFF \ - -DUSE_SOCKET_PUBLISHER=OFF \ - -DUSE_STACK_TRACE_LOGGER=ON \ - -DBOW_FRAMEWORK=DBoW2 \ - -DBUILD_TESTS=OFF \ - .. && \ - make -j${NUM_THREADS} && \ - rm -rf CMakeCache.txt CMakeFiles Makefile cmake_install.cmake example src && \ - chmod -R 777 ./* - -# Copy SRCP2 env to workspace -COPY ./srcp2-competitors /srcp2-competitors/ - -# OpenVSLAMROS -WORKDIR /openvslam/ros/1/ -RUN /bin/bash -c 'source /srcp2-competitors/ros_workspace/install/setup.bash; cd /openvslam/ros/1/; catkin_make \ - -DBUILD_WITH_MARCH_NATIVE=OFF \ - -DUSE_PANGOLIN_VIEWER=OFF \ - -DUSE_SOCKET_PUBLISHER=OFF \ - -DUSE_STACK_TRACE_LOGGER=ON \ - -DBOW_FRAMEWORK=DBoW2' - -SHELL ["/bin/bash", "-c"] - -RUN apt-get update && apt-get install -y python-pip python3-pip openssh-client vim less -RUN python -m pip install pyzmq - -RUN /usr/bin/python3 -m pip install scikit-build -RUN /usr/bin/python3 -m pip install msgpack pyzmq shapely scipy -RUN /usr/bin/python3 -m pip install --install-option="-j$(nproc)" "opencv-python>=3.4.10,<3.4.11" - -RUN mkdir -p /rover_workspace/src - -# Alien Control -WORKDIR /rover_workspace/src -RUN set -x && \ - git clone https://github.com/frantisekbrabec/aliencontrol.git - -COPY ./osgar/moon/ros_launchfiles /rover_workspace/src/aliencontrol/launch -RUN chmod 777 /rover_workspace - -RUN rm -rf /rover_workspace/devel -RUN rm -rf /rover_workspace/build - -RUN /bin/bash -c "source /openvslam/ros/1/devel/setup.bash; cd /rover_workspace; catkin_make" - -RUN echo "source /rover_workspace/devel/setup.bash" >> /root/.bashrc - -COPY ./osgar /osgar/ - -RUN rm -rf /osgar/.git -RUN rm -rf /osgar/.github -RUN rm -rf /osgar/research_scripts -RUN rm -rf /osgar/disparity_images - -WORKDIR / -ENTRYPOINT ["/osgar/moon/docker/rover/entrypoint.bash"] -CMD ["/bin/bash"] diff --git a/moon/docker/rover/entrypoint.bash b/moon/docker/rover/entrypoint.bash deleted file mode 100755 index 593647203..000000000 --- a/moon/docker/rover/entrypoint.bash +++ /dev/null @@ -1,6 +0,0 @@ -#!/bin/bash -set -e - -source /rover_workspace/devel/setup.bash - -exec "$@" diff --git a/moon/docker/rover/rosconsole.config b/moon/docker/rover/rosconsole.config deleted file mode 100644 index 554d832e4..000000000 --- a/moon/docker/rover/rosconsole.config +++ /dev/null @@ -1,9 +0,0 @@ -# -# rosconsole will find this file by default at $ROS_ROOT/config/rosconsole.config -# -# You can define your own by e.g. copying this file and setting -# ROSCONSOLE_CONFIG_FILE (in your environment) to point to the new file -# -log4j.logger.ros=DEBUG -log4j.logger.ros.roscpp.superdebug=WARN -log4j.logger.ros.roscpp.cached_parameters=INFO diff --git a/moon/docker/rover/run_solution.bash b/moon/docker/rover/run_solution.bash deleted file mode 100755 index 89d3db1c0..000000000 --- a/moon/docker/rover/run_solution.bash +++ /dev/null @@ -1,116 +0,0 @@ -#!/usr/bin/env bash - -RUN_UNPAUSED=0 - -while getopts uhr: arg; do - case $arg in - r) - case $OPTARG in - "1" ) - JSONFILES=("moon-round1.json") - ROVERSCRIPTS=("rospy_scout_round1.py --robot_name=scout_1 --push_port=5555 --pull_port=5556 --reqrep_port=5557") - DAEMONS=("rosrun openvslam run_slam -v /openvslam/orb_vocab.dbow2 -c /openvslam/rover_camera_config.yaml -r scout_1") - ;; - "2" ) - JSONFILES=("moon-excavator-round2.json" "moon-hauler-round2.json") - ROVERSCRIPTS=("rospy_excavator_round2.py --robot_name=excavator_1 --push_port=5555 --pull_port=5556 --reqrep_port=5557" "rospy_hauler_round2.py --robot_name=hauler_1 --push_port=6555 --pull_port=6556 --reqrep_port=6557") - DAEMONS=("rosrun openvslam run_slam -v /openvslam/orb_vocab.dbow2 -c /openvslam/rover_camera_config.yaml -r excavator_1") - ;; - "3" ) - JSONFILES=("moon-round3.json") - ROVERSCRIPTS=("rospy_scout_round3.py --robot_name=scout_1 --push_port=5555 --pull_port=5556 --reqrep_port=5557") - DAEMONS=() - ;; - - esac - ;; - h) - echo "-r [1|2|3] to choose the run to execute" - echo "-u run unpaused" - exit - ;; - u) - RUN_UNPAUSED=1 - ;; - *) - exit - ;; - esac -done - -if [[ $RUN_UNPAUSED -eq 1 ]]; then - echo "Unpause simulation in 5 sec" - ( sleep 5; rosservice call /gazebo/unpause_physics "{}" ) & -fi - -ROBOT_PIDS=() -ROS_PIDS=() -DAEMON_PIDS=() - -echo "Start robot solution (no timeout)" -export OSGAR_LOGS=`pwd` -cd /osgar -for s in ${JSONFILES[@]}; do - echo "starting recording of $s" - python3 -m osgar.record moon/config/$s --note "collect some ROS data" & - ROBOT_PIDS+=($!) -done - - -for ((i=0; i < ${#DAEMONS[@]}; i++)) do - echo "Starting daemon '${DAEMONS[$i]}'" - ${DAEMONS[$i]} & - DAEMON_PIDS+=($!) -done - - -cd .. - -# get directory where this bash script lives -samedir=$(dirname $(readlink -f "${BASH_SOURCE[0]}")) - -# enable ROS DEBUG output to see if messages are being dropped -export ROSCONSOLE_CONFIG_FILE="${samedir}/rosconsole.config" - -# Run your solution and wait for ROS master -# http://wiki.ros.org/roslaunch/Commandline%20Tools#line-45 -## roslaunch subt_seed x1.launch --wait & - -for ((i=0; i < ${#ROVERSCRIPTS[@]}; i++)) do - echo "Starting script '${ROVERSCRIPTS[$i]}'" - python /osgar/moon/rospy/${ROVERSCRIPTS[$i]} & - ROS_PIDS+=($!) -done - -# Turn everything off in case of CTRL+C and friends. -function shutdown { - for p in ${ROBOT_PIDS[@]}; do - kill $p - done - for p in ${DAEMON_PIDS[@]}; do - kill $p - done - for p in ${ROS_PIDS[@]}; do - kill $p - done - - wait - exit -} -trap shutdown SIGHUP SIGINT SIGTERM - - -# Wait for the controllers to finish. -for r in ${ROBOT_PIDS[@]}; do - wait $r -done - - -echo "Sleep and finish" -sleep 30 - -# Take robot simulation down. -for r in ${ROS_PIDS[@]}; do - wait $r -done -wait diff --git a/moon/howto.md b/moon/howto.md deleted file mode 100644 index 93f2856fa..000000000 --- a/moon/howto.md +++ /dev/null @@ -1,14 +0,0 @@ -In one terminal start simulation: - -git/srcp2-competitors$ docker/scripts/launch/roslaunch_docker --run-round 1 -n -(-n = no GUI does not work at the moment, because it does not start the simulation, i.e. this is "for later") - -In the second terminal first build image: -git/osgar/moon/docker$ ./build.bash rover - -and then run it: -docker run --network=host -it --rm rover:latest - -Note, that you can also use interactive way via: -docker run --network=host -it --rm rover:latest /bin/bash - diff --git a/moon/light_manager.py b/moon/light_manager.py deleted file mode 100644 index fa38d6b89..000000000 --- a/moon/light_manager.py +++ /dev/null @@ -1,40 +0,0 @@ -""" - Light Manager -""" - -import cv2 -import numpy as np - -from moon.moonnode import MoonNode - -class LightManager(MoonNode): - def __init__(self, config, bus): - super().__init__(config, bus) - self.light_intensity = 0.0 - - def on_left_image(self, data): - limg = cv2.imdecode(np.frombuffer(data, dtype=np.uint8), cv2.IMREAD_COLOR) - CAMERA_HEIGHT,CAMERA_WIDTH, _ = limg.shape - hsv = cv2.cvtColor(limg, cv2.COLOR_BGR2HSV) - mask = np.zeros((CAMERA_HEIGHT,CAMERA_WIDTH), np.uint8) - circle_mask = cv2.circle(mask,(CAMERA_HEIGHT//2,CAMERA_WIDTH//2),200,(255,255,255),thickness=-1) - hist = cv2.calcHist([limg],[2],circle_mask,[256],[0,256]) - topthird = hist[170:] - brightness = int(sum(topthird) / len(topthird)) - if self.debug: - print (self.sim_time, "LightManager: Current brightness: %d" % brightness) - if brightness < 300: - self.light_intensity = min(1.0, self.light_intensity + 0.1) - self.send_request('set_light_intensity %s' % str(self.light_intensity)) - if self.debug: - print (self.sim_time, "LightManager: Increasing light to: %.1f" % self.light_intensity) - elif brightness > 500: - self.light_intensity = max(0.0, self.light_intensity - 0.1) - self.send_request('set_light_intensity %s' % str(self.light_intensity)) - if self.debug: - print (self.sim_time, "LightManager: Lowering light to: %.1f" % self.light_intensity) - - - - -# vim: expandtab sw=4 ts=4 diff --git a/moon/monitors.py b/moon/monitors.py deleted file mode 100644 index 57235b887..000000000 --- a/moon/monitors.py +++ /dev/null @@ -1,80 +0,0 @@ -""" - Moon Monitors for easier strategy coding -""" -import math - -from osgar.lib.mathex import normalizeAnglePIPI - - -def min_dist(laser_data): - if len(laser_data) > 0: - # remove ultra near reflections and unlimited values == 0 - laser_data = [x if x > 10 else 10000 for x in laser_data] - return min(laser_data)/1000.0 - return 0 - - -class LidarCollisionException(Exception): - pass - - -class VirtualBumperException(Exception): - pass - - -class PitchRollException(Exception): - pass - - -class BaseMonitor: - def __init__(self, robot): - self.robot = robot - - def update(self, robot, channel, data): - pass - - # context manager functions - def __enter__(self): - self.callback = self.robot.register(self.update) - return self - - def __exit__(self, exc_type, exc_val, exc_tb): - self.robot.unregister(self.callback) - - -class LidarCollisionMonitor(BaseMonitor): - def update(self, robot, channel, data): - if channel == 'scan': - size = len(robot.scan) - # measure distance in front of the rover = 180deg of 270deg - if min_dist(robot.scan[size//6:-size//6]) < 1.0: - raise LidarCollisionException() - - -class VirtualBumperMonitor(BaseMonitor): - def __init__(self, robot, virtual_bumper): - super().__init__(robot) - self.virtual_bumper = virtual_bumper - - def update(self, robot, channel, data): - if channel == 'desired_speed': - assert len(data) == 2, data - speed, angular_speed = data[0]/1000.0, math.radians(data[1]/100.0) - self.virtual_bumper.update_desired_speed(speed, angular_speed) - elif channel == 'pose2d': - x, y, heading = data - pose = (x / 1000.0, y / 1000.0, math.radians(heading / 100.0)) - self.virtual_bumper.update_pose(robot.time, pose) # hmm, the time here is not nice - if self.virtual_bumper.collision(): - raise VirtualBumperException() - - -class PitchRollMonitor(BaseMonitor): - def update(self, robot, channel, data): - # TODO use orientation with quaternion instead - if channel == 'rot': - yaw, pitch, roll = [normalizeAnglePIPI(math.radians(x/100)) for x in data] - if pitch > 0.5: - raise PitchRollException() - -# vim: expandtab sw=4 ts=4 diff --git a/moon/moon-view.bat b/moon/moon-view.bat deleted file mode 100644 index 19e3b4836..000000000 --- a/moon/moon-view.bat +++ /dev/null @@ -1 +0,0 @@ -python -m osgar.tools.lidarview --pose2d app.pose2d --lidar rosmsg.scan --camera rosmsg.left_image --camera2 rosmsg.right_image --lidar-limit 20000 --window-size 1200 660 --title rosmsg.rot %* diff --git a/moon/moon-view.sh b/moon/moon-view.sh deleted file mode 100755 index e38c7d363..000000000 --- a/moon/moon-view.sh +++ /dev/null @@ -1 +0,0 @@ -python3 -m osgar.tools.lidarview --pose2d app.pose2d --lidar rosmsg.scan --camera rosmsg.left_image --camera2 rosmsg.right_image --lidar-limit 20000 --window-size 1200 660 --title rosmsg.rot $1 diff --git a/moon/moonnode.py b/moon/moonnode.py deleted file mode 100644 index 2528985c5..000000000 --- a/moon/moonnode.py +++ /dev/null @@ -1,61 +0,0 @@ -""" - MoonNode - parent for Moon processing nodes -""" -from datetime import timedelta -from random import Random - -from osgar.node import Node - -# project constants -CAMERA_WIDTH = 640 -CAMERA_HEIGHT = 480 -CAMERA_FOCAL_LENGTH = 381 -CAMERA_BASELINE = 0.42 # meters -WHEEL_RADIUS = 0.275 # meters -WHEEL_SEPARATION_WIDTH = 1.87325 # meters -WHEEL_SEPARATION_LENGTH = 1.5748 # meters -LIDAR_BEAM_SPACING = 0.0262626260519 # angle difference in radians between two consecutive LIDAR beams - -class MoonNode(Node): - def __init__(self, config, bus): - super().__init__(config, bus) - bus.register("request") - - self.rand = Random(0) - self.sim_time = None - self.monitors = [] - self.requests = {} - self.debug = config.get('debug', False) - - def on_sim_clock(self, data): - self.sim_time = timedelta(seconds=data[0], microseconds=data[1]) - - def on_response(self, data): - token, response = data - #print(self.sim_time, "controller:response received: token=%s, response=%s" % (token, response)) - if token in self.requests.keys(): - callback = self.requests[token] - self.requests.pop(token) - if callback is not None: - callback(response) - - def send_request(self, cmd, callback=None): - """Send ROS Service Request from a single place""" - token = hex(self.rand.getrandbits(128)) - self.requests[token] = callback - #print(self.sim_time, "controller:send_request:token: %s, command: %s" % (token, cmd)) - self.publish('request', [token, cmd]) - - def update(self): - channel = super().update() - - handler = getattr(self, "on_" + channel, None) - if handler is not None: - handler(getattr(self, channel)) - - for m in self.monitors: - m(self, channel) - - return channel - -# vim: expandtab sw=4 ts=4 diff --git a/moon/motorpid.py b/moon/motorpid.py deleted file mode 100644 index a15f7a8e9..000000000 --- a/moon/motorpid.py +++ /dev/null @@ -1,21 +0,0 @@ -""" - Motor PID controller -""" - -class MotorPID: - def __init__(self, p=1, i=0, d=0): - self.desired_speed = 0.0 - self.current_speed = 0.0 - self.param_p = p - - def set_desired_speed(self, speed): - self.desired_speed = speed - - def update(self, current_speed): - self.current_speed = current_speed - - def get_effort(self): - err = self.desired_speed - self.current_speed - return self.param_p * err - -# vim: expandtab sw=4 ts=4 diff --git a/moon/odometry.py b/moon/odometry.py deleted file mode 100644 index 8c4bdbe06..000000000 --- a/moon/odometry.py +++ /dev/null @@ -1,72 +0,0 @@ -""" - Odometry - handle odometry for Moon rovers -""" -import math - -# TODO remove duplicity from moon\vehicles\rover.py !!! -WHEEL_RADIUS = 0.275 # meters -WHEEL_SEPARATION_WIDTH = 1.87325 # meters -WHEEL_SEPARATION_LENGTH = 1.5748 # meters - -WHEEL_NAMES = [b'fl', b'fr', b'bl', b'br'] - - -class Odometry: - def __init__(self): - self.pose2d = 0, 0, 0 - self.prev_position = None - self.crab_limit = math.radians(5) - self.turn_in_place_limit = math.radians(30) - - def is_crab_step(self, steering): - avg = sum(steering)/4 - offset = max([abs(s - avg) for s in steering]) - return offset < self.crab_limit - - def is_turn_in_place(self, steering): - fl, fr, bl, br = steering - lim = self.turn_in_place_limit - return fl < -lim and fr > lim and bl > lim and br < -lim - - def update_joint_position(self, names, data): - self.joint_name = names - assert self.joint_name is not None - if self.prev_position is None: - self.prev_position = data - - diff = [b - a for a, b in zip(self.prev_position, data)] - steering = [data[names.index(n + b'_steering_arm_joint')] - for n in WHEEL_NAMES] - x, y, heading = self.pose2d - drive = [diff[names.index(n + b'_wheel_joint')] - for n in WHEEL_NAMES] - if self.is_crab_step(steering): - dist = WHEEL_RADIUS * sum(drive)/4 - # TODO use rather abs min distance with sign - angle = sum(steering)/4 # all wheels point the same direction - x += math.cos(heading + angle) * dist - y += math.sin(heading + angle) * dist - # expected no change of heading - elif self.is_turn_in_place(steering): - # not expected change in x and y coordinates - # average distance driven on wheel on the circle - dist = WHEEL_RADIUS * (-drive[0] + drive[1] - drive[2] + drive[3])/4 - radius = math.hypot(WHEEL_SEPARATION_WIDTH/2, WHEEL_SEPARATION_LENGTH/2) - heading += dist/radius - else: - # measure odometry from rear wheels - name = b'bl_wheel_joint' - name2 = b'br_wheel_joint' - left = WHEEL_RADIUS * diff[self.joint_name.index(name)] - right = WHEEL_RADIUS * diff[self.joint_name.index(name2)] - dist = (left + right)/2 - angle = (right - left)/WHEEL_SEPARATION_WIDTH - x += math.cos(heading) * dist - y += math.sin(heading) * dist - heading += angle - self.prev_position = data[:] - self.pose2d = x, y, heading - return self.pose2d - - -# vim: expandtab sw=4 ts=4 diff --git a/moon/ros_launchfiles/qual_round_1.launch b/moon/ros_launchfiles/qual_round_1.launch deleted file mode 100644 index e3997f32a..000000000 --- a/moon/ros_launchfiles/qual_round_1.launch +++ /dev/null @@ -1,5 +0,0 @@ - - - - - diff --git a/moon/ros_launchfiles/qual_round_2.launch b/moon/ros_launchfiles/qual_round_2.launch deleted file mode 100644 index b5f5ee088..000000000 --- a/moon/ros_launchfiles/qual_round_2.launch +++ /dev/null @@ -1,5 +0,0 @@ - - - - - diff --git a/moon/ros_launchfiles/qual_round_3.launch b/moon/ros_launchfiles/qual_round_3.launch deleted file mode 100644 index 840258e35..000000000 --- a/moon/ros_launchfiles/qual_round_3.launch +++ /dev/null @@ -1,5 +0,0 @@ - - - - - diff --git a/moon/rospy/rospy_base.py b/moon/rospy/rospy_base.py deleted file mode 100644 index 60e3baa98..000000000 --- a/moon/rospy/rospy_base.py +++ /dev/null @@ -1,200 +0,0 @@ -#!/usr/bin/python -""" - Wait for all necessary ROS sensors - this is Python 2.7 code -""" -import time -import struct -import math -from io import BytesIO - -import threading -from threading import RLock -from threading import Thread - -import signal - -import zmq -import sys, getopt - -import rospy -from rosgraph_msgs.msg import Clock -from std_msgs.msg import String - -interrupted = False - - -def signal_handler(signum, frame): - global interrupted - interrupted = True - - -class RospyBasePushPull(Thread): - def __init__(self, argv): - Thread.__init__(self) - try: - opts, args = getopt.getopt(argv, '', ['robot_name=', 'push_port=', 'pull_port=', 'reqrep_port=']) - except getopt.GetoptError as e: - print ("rospy_base.py --push_port --pull_port=; " + str(e)) - sys.exit(2) - - for opt, arg in opts: - if opt in ['--push_port']: - self.PUSH_PORT = arg - elif opt in ['--pull_port']: - self.PULL_PORT = arg - - self.pull_socket = None - - self.g_socket = None - self.g_lock = RLock() - self.prev_time = None - self.downsample_topic_count = {} - self.logging_level = ["INFO"] # VERBOSE, INFO, WARNING, ERROR - - def setup_sockets(self, context=None): - - with self.g_lock: - context = context or zmq.Context() - self.g_socket = context.socket(zmq.PUSH) - self.g_socket.setsockopt(zmq.LINGER, 0) # milliseconds - self.g_socket.bind('tcp://*:' + self.PUSH_PORT) - - context2 = context or zmq.Context() - self.pull_socket = context2.socket(zmq.PULL) - self.pull_socket.setsockopt(zmq.LINGER, 0) # milliseconds - self.pull_socket.RCVTIMEO = 2000 - self.pull_socket.bind('tcp://*:' + self.PULL_PORT) - - def register_handlers(self): - rospy.Subscriber('/clock', Clock, self.callback_clock) - rospy.Subscriber('/osgar/broadcast', String, self.callback_topic, '/osgar/broadcast') - - def process_message(self, message): - pass - - def socket_send(self, data): - assert self.g_socket is not None - with self.g_lock: - self.g_socket.send(data) - - def callback_clock(self, data): - if (self.prev_time is not None and - self.prev_time.nsecs//100000000 != data.clock.nsecs//100000000): - # run at 10Hz, i.e. every 100ms - s1 = BytesIO() - data.serialize(s1) - to_send = s1.getvalue() - header = struct.pack('" % command) - self.osgar_broadcast_msg.data = command - self.osgar_broadcast_pub.publish(self.osgar_broadcast_msg) - return 'OK' - return '' - - def register_handlers(self): - self.osgar_broadcast_pub = rospy.Publisher('/osgar/broadcast', String, queue_size=1, latch=True) - self.osgar_broadcast_msg = String() - - def run(self): - while True: - try: - message = self.reqrep_socket.recv() - response = self.process_message(message) - self.reqrep_socket.send(response) - except zmq.Again as e: - pass - if interrupted: - self.reqrep_socket.close() - break - -class RospyBaseHelper(object): - - def launch(self, pushpullclass, reqrepclass, argv): - - signal.signal(signal.SIGTERM, signal_handler) #SIGTERM - kill - signal.signal(signal.SIGINT, signal_handler) #SIGINT - ctrl-c - rospy.init_node('listener', anonymous=True) - context = zmq.Context.instance() - - ppt = pushpullclass(argv) - ppt.setup_sockets(context) - ppt.register_handlers() - ppt.start() - - rrt = reqrepclass(argv) - rrt.setup_sockets(context) - rrt.register_handlers() - rrt.start() - - while True: - time.sleep(100) # gives room to signal capture - if interrupted: - break - - ppt.join() - rrt.join() - - context.term() - -class RospyBase(RospyBaseHelper): - pass - - -# vim: expandtab sw=4 ts=4 diff --git a/moon/rospy/rospy_excavator.py b/moon/rospy/rospy_excavator.py deleted file mode 100644 index 84b9b3b83..000000000 --- a/moon/rospy/rospy_excavator.py +++ /dev/null @@ -1,59 +0,0 @@ -""" - Proxy for ROS sensors and effectors - this is Python 2.7 code -""" - -import sys -import rospy - -from std_msgs.msg import * # Float64, JointState -from srcp2_msgs.msg import ExcavatorMsg - -from rospy_rover import RospyRover, RospyRoverReqRep, RospyRoverPushPull - -class RospyExcavatorPushPull(RospyRoverPushPull): - - def register_handlers(self): - super(RospyExcavatorPushPull, self).register_handlers() - - QSIZE = 10 - - self.mount_joint_publisher = rospy.Publisher('/' + self.robot_name + '/mount_joint_controller/command', Float64, queue_size=QSIZE) - self.basearm_joint_publisher = rospy.Publisher('/' + self.robot_name + '/basearm_joint_controller/command', Float64, queue_size=QSIZE) - self.distalarm_joint_publisher = rospy.Publisher('/' + self.robot_name + '/distalarm_joint_controller/command', Float64, queue_size=QSIZE) - self.bucket_joint_publisher = rospy.Publisher('/' + self.robot_name + '/bucket_joint_controller/command', Float64, queue_size=QSIZE) - - - self.bucket_msg = Float64() - self.bucket_msg.data = 0 - - rospy.Subscriber('/' + self.robot_name + '/bucket_info', ExcavatorMsg, self.callback_topic, '/' + self.robot_name + '/bucket_info') - - def process_message(self, message): - super(RospyExcavatorPushPull, self).process_message(message) - message_type = message.split(" ")[0] - if message_type == "bucket_position": - mount = float(message.split(" ")[1]) - basearm = float(message.split(" ")[2]) - distalarm = float(message.split(" ")[3]) - bucket = float(message.split(" ")[4]) - - for pub, value in zip( - [self.mount_joint_publisher, self.basearm_joint_publisher, self.distalarm_joint_publisher, self.bucket_joint_publisher], - (mount, basearm, distalarm, bucket)): - self.bucket_msg.data = value - pub.publish(self.bucket_msg) - - - -class RospyExcavatorReqRep(RospyRoverReqRep): - pass - -class RospyExcavator(RospyRover): - pass - -if __name__ == '__main__': - re = RospyExcavator() - re.launch(RospyExcavatorPushPull, RospyExcavatorReqRep, sys.argv[1:]) - -# vim: expandtab sw=4 ts=4 diff --git a/moon/rospy/rospy_excavator_round2.py b/moon/rospy/rospy_excavator_round2.py deleted file mode 100644 index f13f20250..000000000 --- a/moon/rospy/rospy_excavator_round2.py +++ /dev/null @@ -1,28 +0,0 @@ -""" - Proxy for ROS sensors and effectors - this is Python 2.7 code -""" - -import sys -import rospy -import yaml - -from rospy_excavator import RospyExcavator, RospyExcavatorReqRep, RospyExcavatorPushPull -from rospy_round2 import RospyRound2, RospyRound2ReqRep, RospyRound2PushPull - -from srcp2_msgs.srv import Qual2VolatilesSrv - -class RospyExcavatorRound2PushPull(RospyExcavatorPushPull, RospyRound2PushPull): - pass - -class RospyExcavatorRound2ReqRep(RospyExcavatorReqRep, RospyRound2ReqRep): - pass - -class RospyExcavatorRound2(RospyExcavator, RospyRound2): - pass - -if __name__ == '__main__': - rs = RospyExcavatorRound2() - rs.launch(RospyExcavatorRound2PushPull, RospyExcavatorRound2ReqRep, sys.argv[1:]) - -# vim: expandtab sw=4 ts=4 diff --git a/moon/rospy/rospy_hauler.py b/moon/rospy/rospy_hauler.py deleted file mode 100644 index 5cbb3e83b..000000000 --- a/moon/rospy/rospy_hauler.py +++ /dev/null @@ -1,29 +0,0 @@ -#!/usr/bin/python -""" - Proxy for ROS sensors and effectors - this is Python 2.7 code -""" - -import sys -import rospy - -from rospy_rover import RospyRover, RospyRoverReqRep, RospyRoverPushPull -from srcp2_msgs.msg import HaulerMsg - -class RospyHaulerPushPull(RospyRoverPushPull): - - def register_handlers(self): - super(RospyHaulerPushPull, self).register_handlers() - rospy.Subscriber('/' + self.robot_name + '/bin_info', HaulerMsg, self.callback_topic, '/' + self.robot_name + '/bin_info') - -class RospyHaulerReqRep(RospyRoverReqRep): - pass - -class RospyHauler(RospyRover): - pass - -if __name__ == '__main__': - rh = RospyHauler() - rh.launch(RospyHaulerPushPull, RospyHaulerReqRep, sys.argv[1:]) - -# vim: expandtab sw=4 ts=4 diff --git a/moon/rospy/rospy_hauler_round2.py b/moon/rospy/rospy_hauler_round2.py deleted file mode 100644 index 651232af8..000000000 --- a/moon/rospy/rospy_hauler_round2.py +++ /dev/null @@ -1,27 +0,0 @@ -""" - Proxy for ROS sensors and effectors - this is Python 2.7 code -""" - -import sys -import rospy -import yaml - -from rospy_hauler import RospyHauler, RospyHaulerReqRep, RospyHaulerPushPull -from rospy_round2 import RospyRound2, RospyRound2ReqRep, RospyRound2PushPull - -class RospyHaulerRound2PushPull(RospyHaulerPushPull, RospyRound2PushPull): - pass - -class RospyHaulerRound2ReqRep(RospyHaulerReqRep, RospyRound2ReqRep): - pass - -class RospyHaulerRound2(RospyHauler, RospyRound2): - pass - - -if __name__ == '__main__': - rs = RospyHaulerRound2() - rs.launch(RospyHaulerRound2PushPull, RospyHaulerRound2ReqRep, sys.argv[1:]) - -# vim: expandtab sw=4 ts=4 diff --git a/moon/rospy/rospy_round1.py b/moon/rospy/rospy_round1.py deleted file mode 100644 index 94680f40e..000000000 --- a/moon/rospy/rospy_round1.py +++ /dev/null @@ -1,33 +0,0 @@ -#!/usr/bin/python -""" - Wait for all necessary ROS sensors - this is Python 2.7 code -""" - -from functools import partial - -import rospy -from rospy_base import RospyBase, RospyBaseReqRep, RospyBasePushPull - -# SRCP2 specific -from srcp2_msgs.msg import Qual1ScoringMsg - - -class RospyRound1PushPull(RospyBasePushPull): - - def register_handlers(self): - super(RospyRound1PushPull, self).register_handlers() - - rospy.Subscriber('/qual_1_score', Qual1ScoringMsg, partial(self.callback_topic, rate=50), '/qual_1_score') - -class RospyRound1ReqRep(RospyBaseReqRep): - pass - -class RospyRound1Helper(RospyBase): - pass - -class RospyRound1(RospyRound1Helper): - pass - - -# vim: expandtab sw=4 ts=4 diff --git a/moon/rospy/rospy_round2.py b/moon/rospy/rospy_round2.py deleted file mode 100644 index ac921444f..000000000 --- a/moon/rospy/rospy_round2.py +++ /dev/null @@ -1,45 +0,0 @@ -""" - Proxy for ROS sensors and effectors - this is Python 2.7 code -""" - -import sys -import rospy -import yaml -from functools import partial - -from rospy_base import RospyBase, RospyBaseReqRep, RospyBasePushPull -from srcp2_msgs.srv import Qual2VolatilesSrv -from srcp2_msgs.msg import Qual2ScoringMsg - -class RospyRound2PushPull(RospyBasePushPull): - - def register_handlers(self): - super(RospyRound2PushPull, self).register_handlers() - rospy.Subscriber('/qual_2_score', Qual2ScoringMsg, partial(self.callback_topic, rate=50), '/qual_2_score') - - -class RospyRound2ReqRep(RospyBaseReqRep): - - def register_handlers(self): - super(RospyRound2ReqRep, self).register_handlers() - self.get_volatile_list = rospy.ServiceProxy('/qual_2_services/volatile_locations', Qual2VolatilesSrv) - - def process_message(self, message): - result = super(RospyRound2ReqRep, self).process_message(message) - if len(result) == 0: - message_type = message.split(" ")[0] - if message_type == "get_volatile_locations": - result = self.get_volatile_list() - vol_list = yaml.safe_load(str(result)) - return ','.join((str(e['x']) + " " + str(e['y'])) for e in vol_list['poses']) - else: - return '' - else: - return result - -class RospyRound2(RospyBase): - pass - - -# vim: expandtab sw=4 ts=4 diff --git a/moon/rospy/rospy_round3.py b/moon/rospy/rospy_round3.py deleted file mode 100644 index 7de543e8e..000000000 --- a/moon/rospy/rospy_round3.py +++ /dev/null @@ -1,95 +0,0 @@ -#!/usr/bin/python -""" - Wait for all necessary ROS sensors - this is Python 2.7 code -""" -from functools import partial - -import rospy -from rospy_base import RospyBase, RospyBaseReqRep, RospyBasePushPull - -from geometry_msgs.msg import Point - -# SRCP2 specific -from srcp2_msgs.msg import Qual3ScoringMsg -from srcp2_msgs.srv import (AprioriLocationSrv, HomeLocationSrv, HomeAlignedSrv) - - -class RospyRound3PushPull(RospyBasePushPull): - - def register_handlers(self): - super(RospyRound3PushPull, self).register_handlers() - rospy.Subscriber('/qual_3_score', Qual3ScoringMsg, partial(self.callback_topic, rate=50), '/qual_3_score') - -class RospyRound3ReqRep(RospyBaseReqRep): - - def process_message(self, message): - # print("rospy_round3 OSGAR:" + message) - result = super(RospyRound3ReqRep, self).process_message(message) - if len(result) == 0: - message_type = message.split(" ")[0] - - if message_type == "artf": - s = message.split()[1:] # ignore "artf" prefix - vol_type = s[0] - if vol_type == 'cubesat': - # Task 3 - x, y, z = [float(a) for a in s[1:]] - pose = Point(x, y, z) - print ("rospy_round3: Reporting %s at position %f %f %f" % (vol_type, x, y, z)) - try: - rospy.wait_for_service('/apriori_location_service', timeout=2.0) - report_artf = rospy.ServiceProxy('/apriori_location_service', AprioriLocationSrv) - result = report_artf(pose) - return 'ok' - except rospy.service.ServiceException as e: - print("rospy_round3: Apriori position response: %s" % str(e)) - return str(e) - except rospy.ROSException as exc: - print("/apriori_location_service not available: " + str(exc)) - return str(exc) - elif vol_type == 'homebase': - # Task 3 - print("rospy_round3: reporting homebase arrival") - rospy.wait_for_service("/arrived_home_service", timeout=2.0) - report_artf = rospy.ServiceProxy('/arrived_home_service', HomeLocationSrv) - try: - result = report_artf(True) - return 'ok' - print("rospy_round3: Homebase arrival service response: %r" % result) - except rospy.service.ServiceException as e: - print("rospy_round3: Homebase arrival service response: Incorrect") - return str(e) - except rospy.ROSException as exc: - print("rospy_round3: /arrived_home_service not available: " + str(exc)) - return str(exc) - elif vol_type == 'homebase_alignment': - # Task 3 - rospy.wait_for_service("/aligned_service", timeout=2.0) - report_artf = rospy.ServiceProxy('/aligned_service', HomeLocationSrv) - try: - result = report_artf(True) - print("rospy_round3: Aligned service response: %r" % result) - return 'ok' - except rospy.service.ServiceException as e: - print("rospy_round3: Aligned service response: Incorrect") - return str(e) - except rospy.ROSException as exc: - print("rospy_round3: /aligned_service not available: " + str(exc)) - return str(exc) - else: - return '' - else: - return '' - else: - return result - - -class RospyRound3Helper(RospyBase): - pass - -class RospyRound3(RospyRound3Helper): - pass - - -# vim: expandtab sw=4 ts=4 diff --git a/moon/rospy/rospy_rover.py b/moon/rospy/rospy_rover.py deleted file mode 100644 index 2b58e87cb..000000000 --- a/moon/rospy/rospy_rover.py +++ /dev/null @@ -1,245 +0,0 @@ -#!/usr/bin/python -""" - Wait for all necessary ROS sensors - this is Python 2.7 code -""" -import time -import struct -import math -from io import BytesIO - -from rospy_base import RospyBase, RospyBaseReqRep, RospyBasePushPull - -import zmq -import sys, getopt - -import rospy -from std_msgs.msg import * # Float64, JointState -from sensor_msgs.msg import * -from nav_msgs.msg import Odometry -from geometry_msgs.msg import Twist, Point, PoseStamped - -# SRCP2 specific - -from srcp2_msgs.srv import (ToggleLightSrv, BrakeRoverSrv, LocalizationSrv, ResetModelSrv) - - -class RospyRoverPushPull(RospyBasePushPull): - def __init__(self, argv): - super(RospyRoverPushPull, self).__init__(argv) - try: - opts, args = getopt.getopt(argv, '', ['robot_name=', 'push_port=', 'pull_port=', 'reqrep_port=']) - except getopt.GetoptError as e: - print ("rospy_rover.py --robot_name=" + str(e)) - sys.exit(2) - for opt, arg in opts: - if opt in ['--robot_name']: - self.robot_name = arg - - self.WHEEL_SEPARATION_WIDTH = 1.87325 # meters - self.WHEEL_SEPARATION_HEIGHT = 1.5748 # meters - - def register_handlers(self): - super(RospyRoverPushPull, self).register_handlers() - - rospy.Subscriber('/' + self.robot_name + '/joint_states', JointState, self.callback_topic, '/' + self.robot_name + '/joint_states') - rospy.Subscriber('/' + self.robot_name + '/laser/scan', LaserScan, self.callback_topic, '/' + self.robot_name + '/laser/scan') - rospy.Subscriber('/' + self.robot_name + '/imu', Imu, self.callback_topic, '/' + self.robot_name + '/imu') - rospy.Subscriber('/' + self.robot_name + '/openvslam/camera_pose', PoseStamped, self.callback_topic, '/' + self.robot_name + '/openvslam/pose') - rospy.Subscriber('/' + self.robot_name + '/openvslam/enabled', Bool, self.callback_topic, '/' + self.robot_name + '/openvslam/enabled') - - QSIZE = 10 - - rospy.Subscriber('/' + self.robot_name + '/camera/left/image_raw/compressed', CompressedImage, self.callback_topic, - '/' + self.robot_name + '/camera/left/image_raw/compressed') - rospy.Subscriber('/' + self.robot_name + '/camera/right/image_raw/compressed', CompressedImage, self.callback_topic, - '/' + self.robot_name + '/camera/right/image_raw/compressed') - - self.speed_msg = Float64() - self.speed_msg.data = 0 - - self.steering_msg = Float64() - self.steering_msg.data = 0 - self.effort_msg = Float64() - self.effort_msg.data = 0 - - # /name/fl_wheel_controller/command - self.vel_fl_publisher = rospy.Publisher('/' + self.robot_name + '/fl_wheel_controller/command', Float64, queue_size=QSIZE) - self.vel_fr_publisher = rospy.Publisher('/' + self.robot_name + '/fr_wheel_controller/command', Float64, queue_size=QSIZE) - self.vel_bl_publisher = rospy.Publisher('/' + self.robot_name + '/bl_wheel_controller/command', Float64, queue_size=QSIZE) - self.vel_br_publisher = rospy.Publisher('/' + self.robot_name + '/br_wheel_controller/command', Float64, queue_size=QSIZE) - - self.steering_fl_publisher = rospy.Publisher('/' + self.robot_name + '/fl_steering_arm_controller/command', Float64, queue_size=QSIZE) - self.steering_fr_publisher = rospy.Publisher('/' + self.robot_name + '/fr_steering_arm_controller/command', Float64, queue_size=QSIZE) - self.steering_bl_publisher = rospy.Publisher('/' + self.robot_name + '/bl_steering_arm_controller/command', Float64, queue_size=QSIZE) - self.steering_br_publisher = rospy.Publisher('/' + self.robot_name + '/br_steering_arm_controller/command', Float64, queue_size=QSIZE) - - self.light_up_pub = rospy.Publisher('/' + self.robot_name + '/sensor_controller/command', Float64, queue_size=QSIZE, latch=True) - self.light_up_msg = Float64() - - def process_message(self, message): - super(RospyRoverPushPull, self).process_message(message) - -# print("OSGAR:" + message) - message_type = message.split(" ")[0] - if message_type == "cmd_rover": - arr = [float(x) for x in message.split()[1:]] - for pub, angle in zip( - [self.steering_fl_publisher, self.steering_fr_publisher, self.steering_bl_publisher, self.steering_br_publisher], - arr[:4]): - self.steering_msg.data = angle - pub.publish(self.steering_msg) - - for pub, effort in zip( - [self.vel_fl_publisher, self.vel_fr_publisher, self.vel_bl_publisher, self.vel_br_publisher], - arr[4:]): - self.effort_msg.data = effort - pub.publish(self.effort_msg) - - elif message_type == "set_cam_angle": - self.light_up_msg.data = float(message.split(" ")[1]) - self.light_up_pub.publish(self.light_up_msg) - - elif message_type == "cmd_vel": - desired_speed = float(message.split(" ")[1]) - desired_angular_speed = float(message.split(" ")[2]) - - if abs(desired_speed) < 0.001 and abs(desired_angular_speed) < 0.001: - self.speed_msg.data = 0 - else: - self.speed_msg.data = 100 # force to move forward, always TODO PID with scale to Nm - self.vel_fl_publisher.publish(self.speed_msg) - self.vel_fr_publisher.publish(self.speed_msg) - self.vel_bl_publisher.publish(self.speed_msg) - self.vel_br_publisher.publish(self.speed_msg) - - if abs(desired_speed) > 0.001: - if abs(desired_angular_speed) > 0.001: - # i.e. turn and go - radius = desired_speed/desired_angular_speed - angle_left = math.atan2(WHEEL_SEPARATION_HEIGHT/2.0, radius - WHEEL_SEPARATION_WIDTH/2.0) - angle_right = math.atan2(WHEEL_SEPARATION_HEIGHT/2.0, radius + WHEEL_SEPARATION_WIDTH/2.0) - else: - angle_left = 0.0 - angle_right = 0.0 - - self.steering_msg.data = angle_left - self.steering_fl_publisher.publish(steering_msg) - self.steering_msg.data = angle_right - self.steering_fr_publisher.publish(steering_msg) - self.steering_msg.data = -angle_left - self.steering_bl_publisher.publish(steering_msg) - self.steering_msg.data = -angle_right - self.steering_br_publisher.publish(steering_msg) - else: - pass # keep steering angles as they are ... - else: - # may be picked up by a subclass - pass - - -class RospyRoverReqRep(RospyBaseReqRep): - def __init__(self, argv): - super(RospyRoverReqRep, self).__init__(argv) - try: - opts, args = getopt.getopt(argv, '', ['robot_name=', 'push_port=', 'pull_port=', 'reqrep_port=']) - except getopt.GetoptError as e: - print ("rospy_rover.py --robot_name=" + str(e)) - sys.exit(2) - for opt, arg in opts: - if opt in ['--robot_name']: - self.robot_name = arg - - - def process_message(self, message): - result = super(RospyRoverReqRep, self).process_message(message) - if len(result) == 0: - # print("rospy_rover OSGAR:" + message) - message_type = message.split(" ")[0] - if message_type == "set_cam_angle": - angle = float(message.split(" ")[1]) - if "INFO" in self.logging_level: - print ("rospy_rover: Setting cam angle to: %f" % angle) - self.light_up_msg.data = angle - self.light_up_pub.publish(self.light_up_msg) - return 'OK' - - elif message_type == "set_brakes": - val = message.split(" ")[1] - if val.startswith("on"): - brake_torque = 400.0 - elif val.startswith("off"): - brake_torque = 10.0 - else: - brake_torque = float(val) - if "INFO" in self.logging_level: - print ("rospy_rover: Setting brakes to: %f" % brake_torque) - self.brakes(brake_torque) - return 'OK' - - elif message_type == "set_light_intensity": - light_level = message.split(" ")[1] - if "VERBOSE" in self.logging_level: - print ("rospy_rover: Setting light intensity to: %s" % light_level) - self.lights(light_level) - return 'OK' - - elif message_type == "reset_model": - if "INFO" in self.logging_level: - print ("rospy_rover: Resetting model") - self.reset_model(True) - return 'OK' - - elif message_type == "vslam_reset": - if "INFO" in self.logging_level: - print ("rospy_rover: Resetting VSLAM map") - self.vslam_command_msg.data = "reset" - self.vslam_command_pub.publish(self.vslam_command_msg) - return 'OK' - - elif message_type == "request_origin": - if "INFO" in self.logging_level: - print "rospy_rover: Requesting true pose" - try: - rospy.wait_for_service('/' + self.robot_name + '/get_true_pose', timeout=2.0) - request_origin = rospy.ServiceProxy('/' + self.robot_name + '/get_true_pose', LocalizationSrv) - p = request_origin(True) - if "INFO" in self.logging_level: - print("rospy_rover: true pose [%f, %f, %f] [%f, %f, %f, %f]" % (p.pose.position.x, p.pose.position.y, p.pose.position.z, p.pose.orientation.x, p.pose.orientation.y, p.pose.orientation.z, p.pose.orientation.w)) - s = "origin %f %f %f %f %f %f %f" % (p.pose.position.x, p.pose.position.y, p.pose.position.z, - p.pose.orientation.x, p.pose.orientation.y, p.pose.orientation.z, p.pose.orientation.w) - return s - except rospy.service.ServiceException as e: - print(e) - return str(e) - else: - return '' - else: - return result - - def register_handlers(self): - super(RospyRoverReqRep, self).register_handlers() - - QSIZE = 10 - - self.lights = rospy.ServiceProxy('/' + self.robot_name + '/toggle_light', ToggleLightSrv) - - self.light_up_pub = rospy.Publisher('/' + self.robot_name + '/sensor_controller/command', Float64, queue_size=QSIZE, latch=True) - self.light_up_msg = Float64() - - self.vslam_command_pub = rospy.Publisher('/' + self.robot_name + '/vslam/command', String, queue_size=1, latch=True) - self.vslam_command_msg = String() - - self.brakes = rospy.ServiceProxy('/' + self.robot_name + '/brake_rover', BrakeRoverSrv) - - self.reset_model = rospy.ServiceProxy('/' + self.robot_name + '/reset_model', ResetModelSrv) - - -class RospyRoverHelper(RospyBase): - pass - -class RospyRover(RospyRoverHelper): - pass - - -# vim: expandtab sw=4 ts=4 diff --git a/moon/rospy/rospy_scout.py b/moon/rospy/rospy_scout.py deleted file mode 100644 index 2b60067fe..000000000 --- a/moon/rospy/rospy_scout.py +++ /dev/null @@ -1,30 +0,0 @@ -""" - Proxy for ROS sensors and effectors - this is Python 2.7 code -""" - -import sys -import rospy - -from rospy_rover import RospyRover, RospyRoverReqRep, RospyRoverPushPull -from srcp2_msgs.msg import VolSensorMsg - - -class RospyScoutPushPull(RospyRoverPushPull): - def __init__(self, argv): - super(RospyScoutPushPull, self).__init__(argv) - - def register_handlers(self): - super(RospyScoutPushPull, self).register_handlers() - - rospy.Subscriber('/' + self.robot_name + '/volatile_sensor', VolSensorMsg, self.callback_topic, '/' + self.robot_name + '/volatile_sensor') - -class RospyScoutReqRep(RospyRoverReqRep): - pass - -class RospyScout(RospyRover): - pass - -if __name__ == '__main__': - rs = RospyScout() - rs.launch(RospyScoutPushPull, RospyScoutReqRep, sys.argv[1:]) diff --git a/moon/rospy/rospy_scout_round1.py b/moon/rospy/rospy_scout_round1.py deleted file mode 100644 index ab1d0df84..000000000 --- a/moon/rospy/rospy_scout_round1.py +++ /dev/null @@ -1,61 +0,0 @@ -""" - Proxy for ROS sensors and effectors - this is Python 2.7 code - - scout enhanced with reporting detection of volatiles (task specific to Round 1 of Qualification) -""" - -import sys -import rospy - -from rospy_scout import RospyScout, RospyScoutReqRep, RospyScoutPushPull -from rospy_round1 import RospyRound1, RospyRound1ReqRep, RospyRound1PushPull - -from srcp2_msgs.msg import Qual1ScoringMsg -from srcp2_msgs.srv import Qual1ScoreSrv -from geometry_msgs.msg import Point - -class RospyScoutRound1PushPull(RospyScoutPushPull, RospyRound1PushPull): - pass - -class RospyScoutRound1ReqRep(RospyScoutReqRep, RospyRound1ReqRep): - - def process_message(self, message): - result = super(RospyScoutRound1ReqRep, self).process_message(message) - if len(result) == 0: - # print("OSGAR:" + message) - message_type = message.split(" ")[0] - if message_type == "artf": - s = message.split()[1:] # ignore "artf" prefix - vol_type = s[0] - - if vol_type in ['ice', 'ethene', 'methane', 'carbon_mono', 'carbon_dio', 'ammonia', 'hydrogen_sul', 'sulfur_dio']: - # Task 1 - x, y, z = [float(a) for a in s[1:]] - pose = Point(x, y, z) - print ("rospy_scout: Reporting %s at position %f %f %f" % (vol_type, x, y, z)) - try: - rospy.wait_for_service("/vol_detected_service", timeout=2.0) - report_artf = rospy.ServiceProxy('/vol_detected_service', Qual1ScoreSrv) - resp = report_artf(pose=pose, vol_type=vol_type) - print ("rospy_scout: Volatile report result: %r" % resp.result) - return 'ok' - except rospy.ServiceException as exc: - print("rospy_scout: /vol_detected_service exception: " + str(exc)) - return str(exc) - except rospy.ROSException as exc: - print("rospy_scout: /vol_detected_service not available: " + str(exc)) - return str(exc) - else: - return '' - else: - return '' - else: - return result - -class RospyScoutRound1(RospyScout, RospyRound1): - pass - -if __name__ == '__main__': - rs = RospyScoutRound1() - rs.launch(RospyScoutRound1PushPull, RospyScoutRound1ReqRep, sys.argv[1:]) diff --git a/moon/rospy/rospy_scout_round3.py b/moon/rospy/rospy_scout_round3.py deleted file mode 100644 index ace66c052..000000000 --- a/moon/rospy/rospy_scout_round3.py +++ /dev/null @@ -1,29 +0,0 @@ -""" - Proxy for ROS sensors and effectors - this is Python 2.7 code - - scout enhanced with reporting detection of volatiles (task specific to Round 1 of Qualification) -""" - -import sys -import rospy - -from rospy_scout import RospyScout, RospyScoutReqRep, RospyScoutPushPull -from rospy_round3 import RospyRound3, RospyRound3ReqRep, RospyRound3PushPull - -from srcp2_msgs.msg import Qual3ScoringMsg -from srcp2_msgs.srv import Qual3ScoreSrv -from geometry_msgs.msg import Point - -class RospyScoutRound3PushPull(RospyScoutPushPull, RospyRound3PushPull): - pass - -class RospyScoutRound3ReqRep(RospyRound3ReqRep, RospyScoutReqRep): - pass - -class RospyScoutRound3(RospyScout, RospyRound3): - pass - -if __name__ == '__main__': - rs = RospyScoutRound3() - rs.launch(RospyScoutRound3PushPull, RospyScoutRound3ReqRep, sys.argv[1:]) diff --git a/moon/test_artifacts.py b/moon/test_artifacts.py deleted file mode 100644 index ab633d85e..000000000 --- a/moon/test_artifacts.py +++ /dev/null @@ -1,67 +0,0 @@ -import unittest -from unittest.mock import MagicMock -from pathlib import Path - -import numpy as np -import cv2 - -from moon.artifacts import ArtifactDetector - -curdir = Path(__file__).parent - -class ArtifactTest(unittest.TestCase): - - def is_in_test(self, detected, valid): - # note: classifier-based detection is not deterministic, ie it finds slightly different matches on different runs - # we list all possible valid values and then test whether the detected values is within the set of valid ones - # also, allow difference of 5 pixels in coordinates and 200 pixels in overall matching pixel count - def comp_one(d, t): - for j in range(1,5): - if abs(d[j] - t[j]) > 5: - return False - return True - - def comp_tuple(d, t): - for i in range(len(t)): - if d[0] != t[i][0]: - continue - if not comp_one(d, t[i]): - continue - if abs(d[5] - t[i][5]) > 400: - continue - if len(d) != len(t[i]): - continue - if len(d) == 7 and abs(d[6] - t[i][6]) > 1: #distance difference more than 1m - continue - return True - return False - - self.assertEqual(len(detected), len(valid)) - for i in range(len(valid)): - self.assertGreaterEqual(len(detected[i]), 6) # color matches have 7 fields (+distance), classifier matches 6 - if not comp_tuple(detected[i], valid[i]): - return False - return True - - - def Xtest_artifacts(self): - - bus = MagicMock() - config = {"estimate_distance": True, "artefacts": ["rover","cubesat","homebase", "basemarker"]} - detector = ArtifactDetector(config, bus) - for i in range(len(detector.detectors)): - detector.detectors[i]["subsequent_detects_required"] = 0 - - with open(str(curdir/'test_data/cube_homebase.jpg'), mode='rb') as img_h: - img = img_h.read() - - # TODO: artifact now also calculates distance from stereo images, use stereo images for unittest - # classifier detector returns one of two matches for this image, allow both - self.assertTrue(self.is_in_test(detector.detect(img, img), [[('cubesat', 143, 24, 35, 40, 356)], [('homebase', 180, 185, 93, 93, 6264), ('homebase', 176, 140, 117, 114, 7158)], [('homebase', 202, 163, 71, 86, 2062, 1.6899802684783936)]])) - - with open(str(curdir/'test_data/basemarker.jpg'), mode='rb') as img_h: - img = img_h.read() - self.assertTrue(self.is_in_test(detector.detect(img, img), [[('basemarker', 122, 240, 135, 36, 2068, 6.632953643798828)], [('homebase', 0, 0, 429, 287, 14012, 1.5621232986450195)], [('rover', 6, 180, 422, 286, 7662, 1.7264463901519775)]])) - - -# vim: expandtab sw=4 ts=4 diff --git a/moon/test_controller.py b/moon/test_controller.py deleted file mode 100644 index 76ba10141..000000000 --- a/moon/test_controller.py +++ /dev/null @@ -1,53 +0,0 @@ -import unittest -from unittest.mock import MagicMock -from datetime import timedelta - -from osgar.bus import Bus - -from moon.controller import SpaceRoboticsChallenge - - -class EchoController(SpaceRoboticsChallenge): - pass - - -echo_data = None - -class ControllerTest(unittest.TestCase): - def test_callback(self): - global echo_data - - def echo_callback(data): - global echo_data - echo_data = data - - config = {} - bus = Bus(MagicMock(write=MagicMock(return_value=timedelta()))) - - echo = EchoController(config, bus=bus.handle('echo')) - - # initialize internal variables, so that wait_for_init() can be skipped - echo.sim_time = timedelta() - echo.last_position = [0, 0, 0] - echo.yaw = 0.0 - - tester = bus.handle('tester') - tester.register("response") - bus.connect("tester.response", "echo.response") - bus.connect("echo.request", "tester.request") - - echo.send_request('hello!', echo_callback) - echo.start() - - _, channel, data = tester.listen() - self.assertEqual(data, ['0xe3e70682c2094cac629f6fbed82c07cd', 'hello!']) - tester.publish('response', data) - - echo.bus.sleep(0.1) - self.assertEqual(echo_data, 'hello!') - - echo.request_stop() - echo.join() - -# vim: expandtab sw=4 ts=4 - diff --git a/moon/test_controller_excavator_round2.py b/moon/test_controller_excavator_round2.py deleted file mode 100644 index a98f02590..000000000 --- a/moon/test_controller_excavator_round2.py +++ /dev/null @@ -1,35 +0,0 @@ -import unittest -from unittest.mock import MagicMock -from datetime import timedelta -import numpy as np -from osgar.bus import Bus - -from moon.controller_excavator_round2 import SpaceRoboticsChallengeExcavatorRound2 - -from osgar.bus import BusShutdownException - -class SpaceRoboticsChallengeExcavatorRound2Test(unittest.TestCase): - - def test_usage(self): - config = {} - bus = Bus(MagicMock(write=MagicMock(return_value=timedelta()))) - r1 = SpaceRoboticsChallengeExcavatorRound2(config, bus=bus.handle('app')) - r1.start() - r1.request_stop() - r1.join() - - - def test_find_next_volatile(self): - # from seed 1500 - config = {'debug': False} - bus = Bus(MagicMock(write=MagicMock(return_value=timedelta()))) - r2e = SpaceRoboticsChallengeExcavatorRound2(config, bus=bus.handle('app')) - - r2e.xyz = [10.394101, -8.620875, 0.786391] - r2e.yaw = 2.767378 - - r2e.vol_list = [[49.6034306594, -45.9196365544], [-40.7186943541, 25.742583001], [52.5927389894, 31.5772572951], [-35.4015899876, 30.9052755259], [28.626252316, -36.5104539519], [50.846455282, 10.1442217972], [49.3141912004, -41.6719396868], [-33.8047498291, -45.7775507432], [-32.6856318871, 30.7811787209], [41.5629764612, 43.1950851664], [-6.56325228377, 46.9468649025], [-5.62467497268, -4.92013629455], [14.3408662367, 17.4889646202], [54.657022764, 54.5170630599], [15.4820006992, 1.24266919623], [1.23307211219, 52.6044006624], [56.128567115, -6.59073868759], [-1.08706993599, 51.8179897589], [-39.812812773, -32.6237207556], [-39.8509161911, -54.5155864259], [15.8911609315, -3.84581195752], [51.9294757594, 14.3942159102], [-40.8507536421, -45.2608876114], [-27.542088652, 57.0], [51.8354318585, 34.5281584002], [57.2500220512, 45.3890817188], [-37.3400408782, 47.8302730305], [-14.2076440605, -29.3234846144], [-56.6504633815, 46.898890127]] - - v = r2e.get_next_volatile() - np.testing.assert_array_equal([-5.62467497268, -4.92013629455], v) -# vim: expandtab sw=4 ts=4 diff --git a/moon/test_controller_round1.py b/moon/test_controller_round1.py deleted file mode 100644 index 0d0788aaf..000000000 --- a/moon/test_controller_round1.py +++ /dev/null @@ -1,21 +0,0 @@ -import unittest -from unittest.mock import MagicMock -from datetime import timedelta - -from osgar.bus import Bus - -from moon.controller_round1 import SpaceRoboticsChallengeRound1 - - -class SpaceRoboticsChallengeRound1Test(unittest.TestCase): - - def test_usage(self): - config = {} - bus = Bus(MagicMock(write=MagicMock(return_value=timedelta()))) - r1 = SpaceRoboticsChallengeRound1(config, bus=bus.handle('app')) - r1.start() - r1.request_stop() - r1.join() - -# vim: expandtab sw=4 ts=4 - diff --git a/moon/test_controller_round3.py b/moon/test_controller_round3.py deleted file mode 100644 index cb887bbca..000000000 --- a/moon/test_controller_round3.py +++ /dev/null @@ -1,89 +0,0 @@ -import unittest -from random import Random -import math - -import numpy as np -from shapely.geometry import LineString -from shapely.geometry import Point - -from moon.controller_round3 import best_fit_circle - -def pol2cart(rho, phi): - x = rho * math.cos(phi) - y = rho * math.sin(phi) - return(x, y) - -def cart2pol(x, y): - rho = np.sqrt(x**2 + y**2) - phi = np.arctan2(y, x) - return(rho, phi) - -def plot_test_case(circle, lines): - circle_center_x, circle_center_y, circle_radius = circle - x_l, y_l = lines - - - fig, ax = plt.subplots() - ax.set_aspect(1) - ax.add_artist(plt.Circle((circle_center_x, circle_center_y), circle_radius)) - - for i in range(len(lines[0])): - plt.plot([0,x_l[i]],[0,y_l[i]]) - plt.axis([-50, 50, -50, 50]) - plt.show() - -has_matplotlib = False - -class Round3CircleTest(unittest.TestCase): - - def test_round3_circle(self): - # generate random points on 2.6rad arc with random radius between 5 and 20 - # view is at (0,0) - # circle is at random point and has random radius - rand = Random(0) - for _ in range(10): - circle_distance = rand.randint(10,20) - circle_angle = rand.random() * 2 * math.pi - radius = rand.randint(2,6) - center_x, center_y = pol2cart(circle_distance, circle_angle) - circle_angle = circle_angle + (1 - rand.random() * 2) * math.pi / 4 - - p = Point(center_x,center_y) - c = p.buffer(radius).boundary - - x_l=[] - y_l=[] - min_index = rand.randint(0,20) - max_index = rand.randint(80,100) - for inc in range(min_index, max_index): - angle = circle_angle - 1.3 + inc * 0.0262626260519 - ox, oy = pol2cart(2*circle_distance, angle) - l = LineString([(0,0), (ox, oy)]) - i = c.intersection(l) - if i.is_empty: - continue - - i_x = i.geoms[0].coords[0][0] - i_y = i.geoms[0].coords[0][1] - # TODO: pick the nearest intersections - dst,ang = cart2pol(i_x, i_y) - # add noise to distance by 5% - dst = dst * (1 + rand.random()/10 - 0.05) - i_x2, i_y2 = pol2cart(dst, ang) - x_l.append(i_x2) - y_l.append(i_y2) - - if has_matplotlib: - plot_test_case((center_x, center_y, radius),(x_l, y_l)) - (cx, cy, cr) = best_fit_circle(x_l, y_l) - np.testing.assert_almost_equal((center_x/200, center_y/200, radius/200), (cx/200, cy/200, cr/200),2) - -if __name__ == '__main__': - # you may need to define your environment variable to point to the main osgar folder - # eg export PYTHONPATH=/osgar - try: - from matplotlib import pyplot as plt - has_matplotlib = True - except ImportError: - has_matplotlib = False - unittest.main() diff --git a/moon/test_data/basemarker.jpg b/moon/test_data/basemarker.jpg deleted file mode 100644 index 331f48112..000000000 Binary files a/moon/test_data/basemarker.jpg and /dev/null differ diff --git a/moon/test_data/cube_homebase.jpg b/moon/test_data/cube_homebase.jpg deleted file mode 100644 index 99da67dc8..000000000 Binary files a/moon/test_data/cube_homebase.jpg and /dev/null differ diff --git a/moon/test_monitors.py b/moon/test_monitors.py deleted file mode 100644 index 7a6015aab..000000000 --- a/moon/test_monitors.py +++ /dev/null @@ -1,62 +0,0 @@ -import unittest -from unittest.mock import MagicMock - -from moon.monitors import (LidarCollisionException, LidarCollisionMonitor, - VirtualBumperMonitor, - PitchRollException, PitchRollMonitor, - min_dist) - - -class MonitorTester: - def __init__(self): - self.monitors = [] - - def register(self, callback): - self.monitors.append(callback) - return callback - - def unregister(self, callback): - assert callback in self.monitors - self.monitors.remove(callback) - - def update(self, channel): - data = getattr(self, channel) - for m in self.monitors: - m(self, channel, data) - - def publish(self, channel, data): - for m in self.monitors: - m(self, channel, data) - - -class MoonMonitorsTest(unittest.TestCase): - - def test_min_dist(self): - self.assertAlmostEqual(min_dist([1000, 2000, 3000]), 1.0) - self.assertAlmostEqual(min_dist([]), 0.0) - self.assertAlmostEqual(min_dist([0]), 10.0) - - def test_lidar_monitor(self): - robot = MonitorTester() - with LidarCollisionMonitor(robot): - robot.scan = [3000]*270 - robot.update('scan') - - def test_virtual_bumper_monitor(self): - robot = MonitorTester() - with VirtualBumperMonitor(robot, virtual_bumper=MagicMock()): - robot.publish('desired_speed', [0, 0]) - - def test_roll_pitch_monitor(self): - robot = MonitorTester() - with PitchRollMonitor(robot): - robot.rot = [0, 0, 0] - robot.update('rot') - - with self.assertRaises(PitchRollException) as err: - with PitchRollMonitor(robot): - robot.rot = [0, 5000, 0] - robot.update('rot') - -# vim: expandtab sw=4 ts=4 - diff --git a/moon/test_motorpid.py b/moon/test_motorpid.py deleted file mode 100644 index 3cfe60ca4..000000000 --- a/moon/test_motorpid.py +++ /dev/null @@ -1,17 +0,0 @@ -import unittest - -from moon.motorpid import MotorPID - - -class MotorPIDTest(unittest.TestCase): - - def test_pid(self): - pid = MotorPID(p=1, i=0, d=0) - pid.set_desired_speed(0.5) - pid.update(current_speed=0) - effort = pid.get_effort() - self.assertEqual(effort, 0.5) - - -# vim: expandtab sw=4 ts=4 - diff --git a/moon/test_odometry.py b/moon/test_odometry.py deleted file mode 100644 index 063df6536..000000000 --- a/moon/test_odometry.py +++ /dev/null @@ -1,87 +0,0 @@ -import unittest -import math - -from moon.odometry import Odometry - - -class MoonOdometryTest(unittest.TestCase): - - def test_usage(self): - odom = Odometry() - self.assertEqual(odom.pose2d, (0, 0, 0)) - - names = [b'bl_arm_joint', b'bl_steering_arm_joint', b'bl_wheel_joint', - b'br_arm_joint', b'br_steering_arm_joint', b'br_wheel_joint', - b'fl_arm_joint', b'fl_steering_arm_joint', b'fl_wheel_joint', - b'fr_arm_joint', b'fr_steering_arm_joint', b'fr_wheel_joint', - b'sensor_joint'] - - data = [0.045406858648845194, 0.004015732023318286, 15.865968131515782, - 0.06465937149762269, -0.004300069050040101, 15.749286284936085, - -0.06585596104286928, -0.002379553028373671, - 16.106778719739687, -0.057778736914743334, - 0.001733177352261528, 15.999004953177929, - -0.00017593211588806668] - - odom.update_joint_position(names, data) - self.assertEqual(odom.pose2d, (0, 0, 0)) - - # current behavior - ignore steering angles - data[names.index(b'bl_wheel_joint')] += 1.0 - data[names.index(b'br_wheel_joint')] += 1.0 - odom.update_joint_position(names, data) - self.assertAlmostEqual(odom.pose2d[0], 0.275/2) - self.assertAlmostEqual(odom.pose2d[1], 0, 4) - self.assertAlmostEqual(odom.pose2d[2], 0) - - def test_is_crab_step(self): - odom = Odometry() - self.assertTrue(odom.is_crab_step([0, 0, 0, 0])) - a = math.radians(45) - self.assertFalse(odom.is_crab_step([a, a, -a, -a])) - - def test_steering(self): - odom = Odometry() - self.assertEqual(odom.pose2d, (0, 0, 0)) - - names = [b'bl_arm_joint', b'bl_steering_arm_joint', b'bl_wheel_joint', - b'br_arm_joint', b'br_steering_arm_joint', b'br_wheel_joint', - b'fl_arm_joint', b'fl_steering_arm_joint', b'fl_wheel_joint', - b'fr_arm_joint', b'fr_steering_arm_joint', b'fr_wheel_joint', - b'sensor_joint'] - - data = [0.0, 0.0, 10.0, - 0.0, 0.0, 10.0, - 0.0, 0.0, 10.0, - 0.0, 0.0, 10.0, - 0.0] - - odom.update_joint_position(names, data) - self.assertEqual(odom.pose2d, (0, 0, 0)) - - data = [0.0, 0.0, 11.0, - 0.0, 0.0, 11.0, - 0.0, 0.0, 11.0, - 0.0, 0.0, 11.0, - 0.0] - - odom.update_joint_position(names, data) - self.assertEqual(odom.pose2d, (0.275, 0, 0)) - - a90 = math.radians(90) - data = [0.0, a90, 12.0, - 0.0, a90, 12.0, - 0.0, a90, 12.0, - 0.0, a90, 12.0, - 0.0] - odom.update_joint_position(names, data) - self.assertEqual(odom.pose2d, (0.275, 0.275, 0)) - - def test_is_turn_in_place(self): - odom = Odometry() - self.assertFalse(odom.is_turn_in_place([0, 0, 0, 0])) - a = math.radians(45) - self.assertTrue(odom.is_turn_in_place([-a, a, a, -a])) - -# vim: expandtab sw=4 ts=4 - diff --git a/moon/vehicles/__init__.py b/moon/vehicles/__init__.py deleted file mode 100644 index 88097618f..000000000 --- a/moon/vehicles/__init__.py +++ /dev/null @@ -1,4 +0,0 @@ - - -from osgar.lib import create_load_tests -load_tests = create_load_tests(__file__) diff --git a/moon/vehicles/excavator.py b/moon/vehicles/excavator.py deleted file mode 100644 index 7e43ef115..000000000 --- a/moon/vehicles/excavator.py +++ /dev/null @@ -1,146 +0,0 @@ -""" - Moon Rover Driver -""" - -# source: (limited access) -# https://gitlab.com/scheducation/srcp2-competitors/-/wikis/Documentation/API/Simulation_API - -# Excavator Arm and Bucket -# excavator_n/bucket_info -# excavator_n/mount_joint_controller/command -# excavator_n/basearm_joint_controller/command -# excavator_n/distalarm_joint_controller/command -# excavator_n/bucket_joint_controller/command - -# Info -# /excavator_n/bucket_info - -# /name/joint_states sensor_msgs/JointStates -# basearm_joint -# bucket_joint -# distalarm_joint -# mount_joint - -from datetime import timedelta - -import math -from osgar.lib.mathex import normalizeAnglePIPI - -from osgar.node import Node -from moon.vehicles.rover import Rover - -def rad_close(a,b): - return [abs(normalizeAnglePIPI(x-y)) < 0.1 for x,y in zip(a,b)] - -def rad_array_close(a, b): - res = all(rad_close(a,b)) - return bool(res) - -class Excavator(Rover): - def __init__(self, config, bus): - super().__init__(config, bus) - bus.register('cmd', 'bucket_cmd') - self.last_command_timestamp = None - - # TODO: account for working on an incline - - self.target_arm_position = None - self.current_arm_position = None - - self.scoop_time = None - self.execute_bucket_queue = [] - self.arm_joint_names = [b'mount_joint', b'basearm_joint', b'distalarm_joint', b'bucket_joint'] - self.bucket_scoop_sequence = ( - # [, [mount, base, distal, bucket]] - # note, even though target angles are in range, the movement may be obstructed by another part of the robot (e.g, camera) - [20, [-0.6, -0.8, 3.2]], # get above scooping position #TODO: no need to move this high, however, need to swing through front of the robot not to hit hauler - [10, [ 0.66, -0.6, 3.2]], # lower to scooping position - [10, [ 0.66, 0.8, 2.5]], # scoop volatiles - [10, [-0.6, -0.2, 3.92]] # lift up bucket with volatiles - ) - self.bucket_drop_sequence = ( - [20, [-0.6, -0.2, 3.92]], # turn towards dropping position - [10, [0, -0.8, 3.92]], # extend arm - [10, [-0.3, -0.8, 3]], # drop - [10, [-0.6, -0.8, 3.2]] # back to neutral/travel position - ) - self.bucket_rotate_sequence = ( - # [, [mount, base, distal, bucket]] - [10, [-0.6, -0.2, 3.92]], - [10, [-0.6, -0.2, 3.92]], - [10, [-0.6, -0.2, 3.92]], - [10, [-0.6, -0.2, 3.92]] - ) - - def send_bucket_position(self, bucket_params): - mount, basearm, distalarm, bucket = bucket_params - if 0.4 <= abs(mount) <= 1.25 or 1.85 <= abs(mount) <= 2.75: - distalarm = min(0.15, distalarm) - self.target_arm_position = [mount, basearm, distalarm, bucket] - s = '%f %f %f %f\n' % (mount, basearm, distalarm, bucket) - self.publish('bucket_cmd', bytes('bucket_position ' + s, encoding='ascii')) - self.last_command_timestamp = self.sim_time - - def on_bucket_dig(self, data): - dig_angle, queue_action = data - dig = [[duration, [dig_angle, *step]] for duration, step in self.bucket_scoop_sequence] - if queue_action == 'reset': - self.execute_bucket_queue = dig - self.scoop_time = None - elif queue_action == 'append': - self.execute_bucket_queue += dig - elif queue_action == 'prepend': - i = len(self.execute_bucket_queue) % 4 - self.execute_bucket_queue[i:i] = dig - elif queue_action == 'standby': # finish existing action, turn in the prescribed direction and clear rest of queue (hold volatiles) - self.scoop_time = None - i = len(self.execute_bucket_queue) % 4 - self.execute_bucket_queue = self.execute_bucket_queue[:i] + [[duration, [dig_angle, *step]] for duration, step in self.bucket_rotate_sequence] - else: - assert False, "Dig command" - - def on_bucket_drop(self, data): - drop_angle, queue_action = data - drop = [[duration, [drop_angle, *step]] for duration, step in self.bucket_drop_sequence] - if queue_action == 'reset': - self.execute_bucket_queue = drop - self.scoop_time = None - elif queue_action == 'append': - self.execute_bucket_queue += drop - elif queue_action == 'prepend': # need to inject in between 4-step sequences, not in the middle of one - i = len(self.execute_bucket_queue) % 4 - self.execute_bucket_queue[i:i] = drop - else: - assert False, "Drop command" - - def on_joint_position(self, data): - super().on_joint_position(data) - self.current_arm_position = [data[self.joint_name.index(n)] for n in self.arm_joint_names] - - def update(self): - channel = super().update() - - # refresh bucket command - if self.target_arm_position is not None and self.last_command_timestamp is not None and self.sim_time - self.last_command_timestamp > timedelta(milliseconds=300): - self.send_bucket_position(self.target_arm_position) - - # TODO: on a slope one should take into consideration current pitch and roll of the robot - if self.sim_time is not None: - if ( - len(self.execute_bucket_queue) > 0 and - ( - self.scoop_time is None or - self.sim_time > self.scoop_time or - self.target_arm_position is None or - rad_array_close(self.target_arm_position, self.current_arm_position) - ) - ): - duration, bucket_params = self.execute_bucket_queue.pop(0) - # print ("bucket_position %f %f %f " % (bucket_params[0], bucket_params[1],bucket_params[2])) - self.send_bucket_position(bucket_params) - self.scoop_time = self.sim_time + timedelta(seconds=duration) - - return channel - - -# vim: expandtab sw=4 ts=4 diff --git a/moon/vehicles/hauler.py b/moon/vehicles/hauler.py deleted file mode 100644 index ce56fb00f..000000000 --- a/moon/vehicles/hauler.py +++ /dev/null @@ -1,25 +0,0 @@ -""" - Moon Hauler Driver -""" - -# source: (limited access) -# https://gitlab.com/scheducation/srcp2-competitors/-/wikis/Documentation/API/Simulation_API - -# Hauler Bin -# /hauler_n/bin_joint_controller/command - -# Info -# /hauler_n/bin_info - - -import math -from osgar.lib.mathex import normalizeAnglePIPI - -from osgar.node import Node -from moon.vehicles.rover import Rover - -class Hauler(Rover): - def __init__(self, config, bus): - super().__init__(config, bus) - -# vim: expandtab sw=4 ts=4 diff --git a/moon/vehicles/rover.py b/moon/vehicles/rover.py deleted file mode 100644 index f0b9f1d68..000000000 --- a/moon/vehicles/rover.py +++ /dev/null @@ -1,305 +0,0 @@ -""" - Moon Rover Driver -""" - -# source: (limited access) -# https://gitlab.com/scheducation/srcp2-competitors/-/wikis/Documentation/API/Simulation_API -# Motor Drive Command Topics -# /name/fl_wheel_controller/command -# /name/fr_wheel_controller/command -# /name/bl_wheel_controller/command -# /name/br_wheel_controller/command - -# Steering Arm Control Topics -# /name/fr_steering_arm_controller/command -# /name/fl_steering_arm_controller/command -# /name/bl_steering_arm_controller/command -# /name/br_steering_arm_controller/command - -# Info -# /name/joint_states sensor_msgs/JointStates -# /name/skid_cmd_vel geometry_msgs/Twist -# /name/get_true_pose - -# Sensors -# /name/laser/scan sensor_msgs/LaserScan -# /name/camera//image_raw sensor_msgs/Image -# /name/imu sensor_msgs/Imu - -# /name/joint_states sensor_msgs/JointStates -# This is a message that holds data to describe the state of a set of torque controlled joints. -# -# The state of each joint (revolute or prismatic) is defined by: -# * the position of the joint (rad or m), -# * the velocity of the joint (rad/s or m/s) and -# * the effort that is applied in the joint (Nm or N). -# -# Each joint is uniquely identified by its name -# The header specifies the time at which the joint states were recorded. All the joint states -# in one message have to be recorded at the same time. - -# sensor_joint -# bl_arm_joint -# bl_steering_arm_joint -# bl_wheel_joint -# br_arm_joint -# br_steering_arm_joint -# br_wheel_joint -# fl_arm_joint -# fl_steering_arm_joint -# fl_wheel_joint -# fr_arm_joint -# fr_steering_arm_joint -# fr_wheel_joint - -# Sensor Joint Controller -# /name/sensor_controller/command - - -import math -from datetime import timedelta - -from osgar.lib.mathex import normalizeAnglePIPI - -from moon.moonnode import MoonNode, WHEEL_RADIUS, WHEEL_SEPARATION_WIDTH, WHEEL_SEPARATION_LENGTH -from moon.motorpid import MotorPID -from moon.odometry import Odometry - -WHEEL_NAMES = ['fl', 'fr', 'bl', 'br'] - -CRAB_ROLL_ANGLE = 0.78 - -class Rover(MoonNode): - def __init__(self, config, bus): - super().__init__(config, bus) - bus.register('cmd', 'odo_pose', 'desired_speeds') - - # general driving parameters - # radius: radius of circle to drive around, "inf" if going straight; 0 if turning round in place - # positive if turning left, negative if turning right - # camera_angle: direction of camera vs tangent to the circle; ie 0 if looking and going straight or doing regular turn; positive it looking sideways to the left; negative if looking sideways to the right - # when turning in place, positive speed turns counterclockwise, negative clockwise - self.drive_radius = float("inf") # in degrees * 100 - self.drive_camera_angle = 0 # in degrees * 100 - self.drive_speed = 0 # -1000 to 1000 (0.. stop, 1000 maximum feasible speed given the type of motion) - - self.joint_name = None # updated via Node.update() - self.debug_arr = [] - self.verbose = False - self.prev_position = None - self.odom = Odometry() - self.roll = 0.0 - self.pitch = 0.0 - self.yaw = 0.0 - self.yaw_offset = None - self.in_driving_recovery = False - self.steering_wait_start = None - self.steering_wait_repeat = None - self.steering_angle = 0.0 - - self.motor_pid = [MotorPID(p=40.0) for __ in WHEEL_NAMES] # TODO tune PID params - - def on_driving_recovery(self, data): - self.in_driving_recovery = data - - def on_desired_speed(self, data): - # self.desired_linear_speed, self.desired_angular_speed = data[0]/1000.0, math.radians(data[1]/100.0) - linear, angular = data # legacy: mutually exclusive, either linear goes straigth or angular turns in place; both 0 is stop - if linear == 0 and angular == 0: - self.drive_radius = self.drive_speed = 0 - elif angular != 0: - self.drive_radius = 0 # turn in place - self.drive_speed = 1000 * angular / (100 * 60) # max angular speed is 60 deg/sec, value provided in 100 multiple - else: # linear is non-zero - self.drive_radius = float("inf") # going straight - self.drive_speed = linear - self.drive_camera_angle = 0 # only 0, positive (looking left 90 degrees when going forward) and negative (right) are supported - - def on_desired_movement(self, data): - # rover will go forward in a circle given: - # circle radius (m) (use float("inf") to go straight) - # angle between the center of the circle and the direction of the camera (angle in 100*degrees, positive if center to the left of the camera); NOTE: currently only the sign matters and will result in looking left and right 90 degrees respectively - # drive_speed: linear speed in 1000* m/s - self.drive_radius, self.drive_camera_angle, self.drive_speed = data - - def on_rot(self, data): - rot = data - (temp_yaw, self.pitch, self.roll) = [normalizeAnglePIPI(math.radians(x/100)) for x in rot] - - if self.yaw_offset is None: - self.yaw_offset = -temp_yaw - self.yaw = temp_yaw + self.yaw_offset - #print ("yaw: %f, pitch: %f, roll: %f" % (self.yaw, self.pitch, self.roll)) - - def on_joint_position(self, data): - # TODO: this only works for going straight, possibly with turning - # this does not work at all for other types of moves such as going sideways - assert self.joint_name is not None - - left_wheel_angle = b'fl_steering_arm_joint' - right_wheel_angle = b'fr_steering_arm_joint' - self.steering_angle = (data[self.joint_name.index(left_wheel_angle)] + data[self.joint_name.index(right_wheel_angle)]) / 2.0 - - self.odom.update_joint_position(self.joint_name, data) - x, y, heading = self.odom.pose2d - self.bus.publish('odo_pose', [round(x * 1000), - round(y * 1000), - round(math.degrees(heading) * 100)]) - self.prev_position = data # TODO remove dependency - - def on_joint_velocity(self, data): - assert self.joint_name is not None - speed = [] - for i, wheel in enumerate(WHEEL_NAMES): # cycle through fl, fr, bl, br - s = WHEEL_RADIUS * data[self.joint_name.index(bytes(wheel, 'ascii') + b'_wheel_joint')] - speed.append(s) - self.motor_pid[i].update(s) - - if self.verbose: - self.debug_arr.append([self.time.total_seconds(),] + speed) - - def on_joint_effort(self, data): - assert self.joint_name is not None - steering, effort = self.get_steering_and_effort() - - ##### integrate PID start ##### - for i, e in enumerate(effort): - self.motor_pid[i].set_desired_speed(e/40) # TODO review values 0, 40, 60, 120 - effort = [] - for m in self.motor_pid: - effort.append(m.get_effort()) - ###### integrate PID end ###### - cmd = b'cmd_rover %f %f %f %f %f %f %f %f' % tuple(steering + effort) - self.bus.publish('cmd', cmd) - - def get_steering_and_effort(self): - movement_type = 'none' - steering = [0.0,] * 4 - - if self.drive_speed == 0: - effort = [0,] * 4 - - elif self.drive_radius == 0: - # turning in place if radius is 0 but speed is non-zero - e = 40 * self.drive_speed / 1000.0 - movement_type = 'angular' - effort = [-e, e, -e, e] - steering = [-CRAB_ROLL_ANGLE,CRAB_ROLL_ANGLE,CRAB_ROLL_ANGLE,-CRAB_ROLL_ANGLE] - - else: - movement_type = 'linear' - - # TODO: if large change of 'steering' values, allow time to apply before turning on 'effort' - fl = fr = rl = rr = 0.0 - - e = 80 * self.drive_speed / 1000.0 - effort = [e, e, e, e] - - if not math.isinf(self.drive_radius): - sign = 1 if self.drive_radius > 0 else -1 - signed_width = -math.copysign(WHEEL_SEPARATION_WIDTH/2.0, self.drive_radius) - fl = sign * WHEEL_SEPARATION_LENGTH / (abs(self.drive_radius) + signed_width) # + if outer - fr = sign * WHEEL_SEPARATION_LENGTH / (abs(self.drive_radius) - signed_width) - rl = sign * -WHEEL_SEPARATION_LENGTH / (abs(self.drive_radius) + signed_width) - rr = sign * -WHEEL_SEPARATION_LENGTH / (abs(self.drive_radius) - signed_width) - - if self.drive_camera_angle == 0: - pass - elif self.drive_camera_angle == 9000: - if self.drive_radius > 0: - temp = rr - rr = -math.pi/2 + fr - fr = -math.pi/2 + fl - fl = math.pi/2 + rl - rl = math.pi/2 + temp - effort = [-e, e, -e, e] - else: - temp = rr - rr = math.pi/2 + fr - fr = math.pi/2 + fl - fl = -math.pi/2 + rl - rl = -math.pi/2 + temp - effort = [e, -e, e, -e] - elif self.drive_camera_angle == -9000: - if self.drive_radius > 0: - temp = rr - rr = math.pi/2 + rl - rl = -math.pi/2 + fl - fl = -math.pi/2 + fr - fr = math.pi/2 + temp - effort = [-e, e, -e, e] - else: - temp = rr - rr = -math.pi/2 + rl - rl = math.pi/2 + fl - fl = math.pi/2 + fr - fr = -math.pi/2 + temp - effort = [e, -e, e, -e] - else: - assert False, "Unsupported angle: " + str(self.drive_camera_angle) - - else: # if driving straight but camera at an angle, point all wheels in the same direction for crab movement - angle = math.radians(self.drive_camera_angle / 100.0) - rr = fr = fl = rl = angle - - steering = [fl, fr, rl, rr] - - WAIT_TO_STEER_MS = 8000 - # stay put while joint angles are catching up - if ( - self.drive_camera_angle != 0 and - self.prev_position is not None and - not self.in_driving_recovery - ): - if ( - abs(self.prev_position[self.joint_name.index(b'bl_steering_arm_joint')] - steering[2]) > 0.2 or - abs(self.prev_position[self.joint_name.index(b'br_steering_arm_joint')] - steering[3]) > 0.2 or - abs(self.prev_position[self.joint_name.index(b'fl_steering_arm_joint')] - steering[0]) > 0.2 or - abs(self.prev_position[self.joint_name.index(b'fr_steering_arm_joint')] - steering[1]) > 0.2 - ): - if ( - (self.steering_wait_start is None or self.sim_time - self.steering_wait_start <= timedelta(milliseconds=WAIT_TO_STEER_MS)) and - - (self.steering_wait_repeat is None or self.sim_time - self.steering_wait_repeat > timedelta(milliseconds=1000)) - ): - if self.steering_wait_start is None: - self.steering_wait_start = self.sim_time - # brake while steering angles are changing so that the robot doesn't roll away while wheels turning meanwhile - # use braking force 20 Nm/rad which should prevent sliding but can be overcome by motor effort - effort = [0.0,]*4 - else: - pass # angles are not reached but attempt timed out, do not change 'effort' - - else: # angles are reached - if self.steering_wait_start is not None: - self.steering_wait_start = None # angles were reached successfully, can wait any time again - self.steering_wait_repeat = None # angles were reached successfully, can wait any time again - - # if attempt to steer without effort timed out, brakes off, start instant repeat prevention timer - if self.steering_wait_start is not None and self.sim_time - self.steering_wait_start > timedelta(milliseconds=WAIT_TO_STEER_MS): - self.steering_wait_start = None - self.steering_wait_repeat = self.sim_time - - effort_sum = sum([abs(x) for x in effort]) - self.bus.publish('desired_speeds', [ - self.drive_speed / 1000.0 if effort_sum > 0 and movement_type == 'linear' else 0.0, - math.copysign(math.radians(30), self.drive_speed) if effort_sum > 0 and movement_type == 'angular' else 0.0, - self.steering_angle - ]) - - return steering, effort - - def draw(self): - # for debugging - import matplotlib.pyplot as plt - arr = self.debug_arr - t = [a[0] for a in arr] - values = [a[1:] for a in arr] - - line = plt.plot(t, values, '-', linewidth=2) - - plt.xlabel('time (s)') - plt.legend(WHEEL_NAMES) - plt.show() - -# vim: expandtab sw=4 ts=4 diff --git a/moon/vehicles/scout.py b/moon/vehicles/scout.py deleted file mode 100644 index 57b16683f..000000000 --- a/moon/vehicles/scout.py +++ /dev/null @@ -1,61 +0,0 @@ -""" - Moon Scout Driver -""" - -# source: (limited access) -# https://gitlab.com/scheducation/srcp2-competitors/-/wikis/Documentation/API/Simulation_API - - -# SCOUT -# Volatile Sensor -# /scout_n/volatile_sensor srcp2_msgs/VolSensorMsg -# sensor delay: rosparam get /volatile_detection_service_delay_range - - -import math -from datetime import timedelta - -from osgar.lib.mathex import normalizeAnglePIPI - -from osgar.node import Node -from moon.vehicles.rover import Rover - - -class Scout(Rover): - def __init__(self, config, bus): - super().__init__(config, bus) - bus.register('object_reached') - self.last_volatile_distance = None - self.last_vol_index = None - self.last_vol_timestamp = None - - def on_volatile(self, data): - - # called by incoming volatile sensor report (among other sources) - # 0 vol_type, 1 distance_to, 2 vol_index - artifact_type = data[0] # meters ... TODO distinguish CubeSat, volatiles, ProcessingPlant - if artifact_type in ['ice', 'ethene', 'methane', 'carbon_mono', 'carbon_dio', 'ammonia', 'hydrogen_sul', 'sulfur_dio']: - distance_to = data[1] - vol_index = data[2] - - if self.last_volatile_distance is None: - self.last_volatile_distance = distance_to - elif self.last_volatile_distance > distance_to: - # print ("Volatile detection %d, getting closer: %f" % (vol_index, distance_to)) - self.last_volatile_distance = distance_to - elif self.last_vol_index is None or self.sim_time - self.last_vol_timestamp > timedelta(seconds=30): - # if no previous volatile or a volatile was last reported more than 30 seconds ago - self.last_vol_index = vol_index - self.last_vol_timestamp = self.sim_time - self.last_volatile_distance = None - # TODO: this must be adjusted to report the position of the sensor, not the robot (which NASA will update their code for at some point) - # the best known distance was in reference to mutual position of the sensor and the volatile - print (self.sim_time, "Volatile detection, starting to go further, reporting") - - self.bus.publish('object_reached', [artifact_type, vol_index]) - else: - self.last_volatile_distance = None - # print ("Previously visited volatile %d, not reporting" % vol_index) - - -# vim: expandtab sw=4 ts=4 diff --git a/moon/vehicles/test_rover.py b/moon/vehicles/test_rover.py deleted file mode 100644 index 57a2350bb..000000000 --- a/moon/vehicles/test_rover.py +++ /dev/null @@ -1,32 +0,0 @@ -import unittest -from unittest.mock import MagicMock - -from moon.vehicles.rover import Rover - - -JOINT_NAME = [b'bl_arm_joint', b'bl_steering_arm_joint', b'bl_wheel_joint', - b'br_arm_joint', b'br_steering_arm_joint', b'br_wheel_joint', b'fl_arm_joint', - b'fl_steering_arm_joint', b'fl_wheel_joint', b'fr_arm_joint', b'fr_steering_arm_joint', - b'fr_wheel_joint', b'sensor_joint'] - - -class RoverTest(unittest.TestCase): - - def test_rover_pid(self): - rover = Rover(config={}, bus=MagicMock()) - - rover.joint_name = JOINT_NAME # in reality received as the first joint status - rover.on_joint_effort([0]*len(JOINT_NAME)) - - def test_get_steering_and_effort(self): - rover = Rover(config={}, bus=MagicMock()) - self.assertEqual(rover.get_steering_and_effort(), ([0.0, 0.0, 0.0, 0.0], [0, 0, 0, 0])) - - rover.drive_speed = 500.0 # sigh, corresponds to 0.5m/s now - self.assertEqual(rover.get_steering_and_effort(), ([0.0, 0.0, 0.0, 0.0], [40, 40, 40, 40])) - - rover.drive_speed = -500.0 - self.assertEqual(rover.get_steering_and_effort(), ([0.0, 0.0, 0.0, 0.0], [-40, -40, -40, -40])) - -# vim: expandtab sw=4 ts=4 - diff --git a/moon/xml/cubesat.xml b/moon/xml/cubesat.xml deleted file mode 100644 index 0d34d6562..000000000 --- a/moon/xml/cubesat.xml +++ /dev/null @@ -1,1653 +0,0 @@ - - - - BOOST - HAAR - 24 - 24 - - GAB - 9.9500000476837158e-01 - 5.0000000000000000e-01 - 9.4999999999999996e-01 - 1 - 100 - - 0 - 1 - BASIC - 20 - - - <_> - 4 - -1.7763788700103760e+00 - - <_> - - 0 -1 65 -1.0731105506420135e-01 - - 8.5579937696456909e-01 -9.3273985385894775e-01 - <_> - - 0 -1 13 2.8603139519691467e-01 - - -8.1640088558197021e-01 8.8167029619216919e-01 - <_> - - 0 -1 103 -8.3997063338756561e-02 - - 8.9098072052001953e-01 -6.1736381053924561e-01 - <_> - - 0 -1 43 -3.3243607729673386e-02 - - 5.9012573957443237e-01 -9.1132384538650513e-01 - - <_> - 4 - -1.6950113773345947e+00 - - <_> - - 0 -1 13 3.9369738101959229e-01 - - -8.6821705102920532e-01 7.5838923454284668e-01 - <_> - - 0 -1 104 -1.1140526831150055e-01 - - 8.8796406984329224e-01 -6.1105650663375854e-01 - <_> - - 0 -1 43 -1.1135376989841461e-01 - - 6.0366022586822510e-01 -7.1630311012268066e-01 - <_> - - 0 -1 57 4.9579285085201263e-02 - - -7.7226412296295166e-01 5.0056535005569458e-01 - - <_> - 3 - -1.0223461389541626e+00 - - <_> - - 0 -1 15 3.1964465975761414e-01 - - -8.4894257783889771e-01 5.1335310935974121e-01 - <_> - - 0 -1 102 -1.1803430318832397e-01 - - 9.0320700407028198e-01 -5.6931126117706299e-01 - <_> - - 0 -1 0 6.1538349837064743e-03 - - 3.9590767025947571e-01 -9.3076860904693604e-01 - - <_> - 3 - -1.4175050258636475e+00 - - <_> - - 0 -1 12 4.3142926692962646e-01 - - -8.2558137178421021e-01 6.1073827743530273e-01 - <_> - - 0 -1 1 -4.8497421666979790e-03 - - -9.5526117086410522e-01 3.1026750802993774e-01 - <_> - - 0 -1 47 8.6994290351867676e-02 - - -7.3735922574996948e-01 3.6333754658699036e-01 - - <_> - 4 - -1.1093946695327759e+00 - - <_> - - 0 -1 67 -1.3155296444892883e-01 - - 7.1830987930297852e-01 -7.3679500818252563e-01 - <_> - - 0 -1 107 -2.3005757611826994e-05 - - 2.5486528873443604e-01 -8.8224047422409058e-01 - <_> - - 0 -1 31 3.3323396928608418e-04 - - -8.4642744064331055e-01 2.7363660931587219e-01 - <_> - - 0 -1 119 3.5726379137486219e-03 - - 2.3600420355796814e-01 -9.7044986486434937e-01 - - <_> - 5 - -1.1959925889968872e+00 - - <_> - - 0 -1 59 1.8601806461811066e-01 - - -8.6328554153442383e-01 2.6713949441909790e-01 - <_> - - 0 -1 87 -1.0422490537166595e-02 - - 1.5851336717605591e-01 -8.5624587535858154e-01 - <_> - - 0 -1 14 8.6870878934860229e-02 - - -5.3293555974960327e-01 5.1731812953948975e-01 - <_> - - 0 -1 52 1.7370024323463440e-01 - - -3.4807187318801880e-01 9.0205365419387817e-01 - <_> - - 0 -1 5 4.3378816917538643e-03 - - 3.8978707790374756e-01 -9.7075486183166504e-01 - - <_> - 4 - -1.1549322605133057e+00 - - <_> - - 0 -1 80 -1.0907375067472458e-01 - - 3.5958003997802734e-01 -8.5036879777908325e-01 - <_> - - 0 -1 5 -1.5457442495971918e-03 - - -9.7117441892623901e-01 1.6264243423938751e-01 - <_> - - 0 -1 33 1.1994346095889341e-05 - - -7.8616952896118164e-01 3.0540341138839722e-01 - <_> - - 0 -1 23 -1.0878052562475204e-01 - - 3.6120754480361938e-01 -6.7757034301757812e-01 - - <_> - 4 - -1.0185103416442871e+00 - - <_> - - 0 -1 67 -1.5240025520324707e-01 - - 5.2331608533859253e-01 -6.7810028791427612e-01 - <_> - - 0 -1 120 -3.9253211580216885e-03 - - -9.5658898353576660e-01 1.9839330017566681e-01 - <_> - - 0 -1 58 -5.6157931685447693e-03 - - 3.8427257537841797e-01 -6.4866739511489868e-01 - <_> - - 0 -1 27 -1.4753897266928107e-04 - - 3.1847560405731201e-01 -9.2307597398757935e-01 - - <_> - 5 - -1.4992480278015137e+00 - - <_> - - 0 -1 25 2.2804471850395203e-01 - - -7.2967678308486938e-01 2.4271844327449799e-01 - <_> - - 0 -1 9 1.2210819870233536e-02 - - -8.9345371723175049e-01 2.0208062231540680e-01 - <_> - - 0 -1 110 -5.0486929714679718e-02 - - 6.8349206447601318e-01 -3.7720257043838501e-01 - <_> - - 0 -1 38 -3.4035141579806805e-03 - - -8.6295527219772339e-01 2.8725942969322205e-01 - <_> - - 0 -1 37 1.6963561065495014e-03 - - 2.6850599050521851e-01 -9.1863429546356201e-01 - - <_> - 4 - -1.1568306684494019e+00 - - <_> - - 0 -1 22 1.5400445461273193e-01 - - -7.3092371225357056e-01 1.7365269362926483e-01 - <_> - - 0 -1 30 -1.0239852126687765e-03 - - -8.6492031812667847e-01 2.2059564292430878e-01 - <_> - - 0 -1 100 -1.9529994460754097e-05 - - 2.1687485277652740e-01 -8.8243705034255981e-01 - <_> - - 0 -1 48 1.0655312240123749e-01 - - -6.8243789672851562e-01 3.2739499211311340e-01 - - <_> - 6 - -1.6585725545883179e+00 - - <_> - - 0 -1 106 -4.8047415912151337e-02 - - 1.5492957830429077e-01 -6.8260037899017334e-01 - <_> - - 0 -1 17 1.0785663107526489e-05 - - -7.1173208951950073e-01 2.5201562047004700e-01 - <_> - - 0 -1 11 1.5632505528628826e-03 - - 1.9625850021839142e-01 -9.4042325019836426e-01 - <_> - - 0 -1 113 2.4057335685938597e-03 - - 2.2514323890209198e-01 -8.4223288297653198e-01 - <_> - - 0 -1 56 6.3886318821460009e-04 - - -6.9536679983139038e-01 2.4094425141811371e-01 - <_> - - 0 -1 29 -1.2840221170336008e-03 - - -8.3730375766754150e-01 1.4079003036022186e-01 - - <_> - 7 - -1.2429510354995728e+00 - - <_> - - 0 -1 64 -2.0276606082916260e-01 - - 1.3479623198509216e-01 -7.0524233579635620e-01 - <_> - - 0 -1 97 -5.2133394638076425e-04 - - 1.7818945646286011e-01 -7.7501225471496582e-01 - <_> - - 0 -1 40 1.2101027095923200e-04 - - -8.1150048971176147e-01 1.6734078526496887e-01 - <_> - - 0 -1 74 -4.5940108597278595e-02 - - 5.5300629138946533e-01 -3.5800576210021973e-01 - <_> - - 0 -1 36 1.2144135311245918e-02 - - 2.1472120285034180e-01 -8.0753040313720703e-01 - <_> - - 0 -1 24 9.4501499552279711e-04 - - -7.5240367650985718e-01 2.6127502322196960e-01 - <_> - - 0 -1 117 -2.9323240742087364e-03 - - -8.5119873285293579e-01 1.5125203132629395e-01 - - <_> - 8 - -1.2547856569290161e+00 - - <_> - - 0 -1 70 -1.9134733080863953e-01 - - 7.7881619334220886e-02 -6.8880081176757812e-01 - <_> - - 0 -1 50 -3.2312935218214989e-05 - - 1.6886511445045471e-01 -7.3210716247558594e-01 - <_> - - 0 -1 41 3.4539559483528137e-01 - - -2.3259815573692322e-01 8.0029100179672241e-01 - <_> - - 0 -1 121 -3.1036352738738060e-03 - - -8.9275604486465454e-01 2.0947994291782379e-01 - <_> - - 0 -1 114 5.2944645285606384e-03 - - 2.0856343209743500e-01 -7.8368622064590454e-01 - <_> - - 0 -1 2 1.5129726671148092e-04 - - -6.2542623281478882e-01 2.1871116757392883e-01 - <_> - - 0 -1 96 -1.2803180143237114e-03 - - 5.5232971906661987e-01 -2.8927743434906006e-01 - <_> - - 0 -1 69 -1.4541880227625370e-03 - - -7.8038209676742554e-01 1.8042202293872833e-01 - - <_> - 10 - -1.6436764001846313e+00 - - <_> - - 0 -1 71 -1.4893685281276703e-01 - - 7.1717172861099243e-01 -6.0194963216781616e-01 - <_> - - 0 -1 91 2.5176435708999634e-01 - - -4.8791027069091797e-01 4.9327927827835083e-01 - <_> - - 0 -1 8 7.4203428812325001e-04 - - 3.1429255008697510e-01 -7.4050301313400269e-01 - <_> - - 0 -1 4 9.1143744066357613e-03 - - -7.6033836603164673e-01 2.2750447690486908e-01 - <_> - - 0 -1 6 1.6196492360904813e-03 - - -7.1707957983016968e-01 1.9459809362888336e-01 - <_> - - 0 -1 77 2.9896192718297243e-03 - - 1.6991698741912842e-01 -8.1399184465408325e-01 - <_> - - 0 -1 76 -3.8351656257873401e-05 - - 2.9960352182388306e-01 -4.8528337478637695e-01 - <_> - - 0 -1 8 -2.2373271640390158e-03 - - -9.5892530679702759e-01 1.3894541561603546e-01 - <_> - - 0 -1 42 -1.3277854304760695e-04 - - 4.3600243330001831e-01 -3.2225269079208374e-01 - <_> - - 0 -1 99 -1.5871037612669170e-04 - - 1.9630497694015503e-01 -6.5627503395080566e-01 - - <_> - 9 - -1.3902519941329956e+00 - - <_> - - 0 -1 71 -1.6101604700088501e-01 - - 6.4285713434219360e-01 -5.8105939626693726e-01 - <_> - - 0 -1 109 -3.8842916488647461e-02 - - 4.9103975296020508e-01 -3.7913420796394348e-01 - <_> - - 0 -1 51 3.5085589624941349e-03 - - 1.8410773575305939e-01 -8.8983482122421265e-01 - <_> - - 0 -1 72 7.0340512320399284e-04 - - -6.9077366590499878e-01 2.0735569298267365e-01 - <_> - - 0 -1 79 -7.5005833059549332e-04 - - 2.1590572595596313e-01 -5.6839936971664429e-01 - <_> - - 0 -1 90 -2.3904861882328987e-03 - - -7.8342694044113159e-01 1.4575712382793427e-01 - <_> - - 0 -1 45 -9.0469624847173691e-03 - - -7.1426594257354736e-01 1.5647538006305695e-01 - <_> - - 0 -1 55 3.9108673809096217e-04 - - -5.6801253557205200e-01 2.2533559799194336e-01 - <_> - - 0 -1 85 -7.1903630159795284e-03 - - 3.7968823313713074e-01 -3.4695786237716675e-01 - - <_> - 9 - -1.1890842914581299e+00 - - <_> - - 0 -1 63 4.7757309675216675e-01 - - -5.7369256019592285e-01 7.9411762952804565e-01 - <_> - - 0 -1 105 -1.6765329241752625e-01 - - 4.6564206480979919e-01 -3.2584646344184875e-01 - <_> - - 0 -1 44 -1.1655101843643934e-05 - - 2.0211565494537354e-01 -6.8869179487228394e-01 - <_> - - 0 -1 101 4.9933639820665121e-04 - - 1.1478327214717865e-01 -9.2296457290649414e-01 - <_> - - 0 -1 39 -1.2094845995306969e-02 - - -6.7035037279129028e-01 1.7894192039966583e-01 - <_> - - 0 -1 35 2.6610383065417409e-04 - - -4.8722130060195923e-01 2.5898674130439758e-01 - <_> - - 0 -1 93 -2.9077546787448227e-04 - - 3.7888914346694946e-01 -3.0513900518417358e-01 - <_> - - 0 -1 9 1.5325201675295830e-02 - - -7.7825880050659180e-01 1.8255625665187836e-01 - <_> - - 0 -1 30 2.4302736856043339e-03 - - 1.0120475292205811e-01 -8.8097196817398071e-01 - - <_> - 9 - -1.3844150304794312e+00 - - <_> - - 0 -1 66 2.8159397840499878e-01 - - -5.8642971515655518e-01 6.0869562625885010e-01 - <_> - - 0 -1 19 -1.2280125916004181e-01 - - 1.0149873048067093e-01 -7.5372302532196045e-01 - <_> - - 0 -1 118 2.8831907548010349e-03 - - 2.0492509007453918e-01 -9.2698740959167480e-01 - <_> - - 0 -1 62 -1.3045424129813910e-03 - - 3.4658113121986389e-01 -3.9505884051322937e-01 - <_> - - 0 -1 115 -2.2695576772093773e-03 - - -8.1865268945693970e-01 2.6868522167205811e-01 - <_> - - 0 -1 16 1.0583614930510521e-03 - - 2.0537315309047699e-01 -5.1469695568084717e-01 - <_> - - 0 -1 78 1.2351528857834637e-04 - - -3.1408834457397461e-01 4.6611621975898743e-01 - <_> - - 0 -1 7 -1.2841483578085899e-02 - - 4.8998966813087463e-01 -3.4929934144020081e-01 - <_> - - 0 -1 60 -6.0142803704366088e-04 - - 6.6168212890625000e-01 -2.1288764476776123e-01 - - <_> - 10 - -1.4029769897460938e+00 - - <_> - - 0 -1 26 2.1233525127172470e-02 - - -6.2435674667358398e-01 3.5365852713584900e-01 - <_> - - 0 -1 112 -3.8325728382915258e-05 - - 1.5820050239562988e-01 -6.1642050743103027e-01 - <_> - - 0 -1 18 5.6623378768563271e-03 - - 1.1930430680513382e-01 -9.3064844608306885e-01 - <_> - - 0 -1 34 -8.3050606772303581e-03 - - -7.4554306268692017e-01 1.4150951802730560e-01 - <_> - - 0 -1 10 -9.2758936807513237e-04 - - 3.5231667757034302e-01 -4.1071215271949768e-01 - <_> - - 0 -1 84 -5.5437023547710851e-05 - - 5.0000518560409546e-01 -2.9451718926429749e-01 - <_> - - 0 -1 88 -7.0270156720653176e-04 - - 5.0240814685821533e-01 -2.4712041020393372e-01 - <_> - - 0 -1 54 7.7250803587958217e-04 - - -2.7628305554389954e-01 4.7125324606895447e-01 - <_> - - 0 -1 21 7.5689540244638920e-04 - - -8.2054769992828369e-01 1.3492652773857117e-01 - <_> - - 0 -1 73 6.3064564019441605e-03 - - 1.8104727566242218e-01 -6.3963425159454346e-01 - - <_> - 12 - -1.4778032302856445e+00 - - <_> - - 0 -1 94 -4.4983979314565659e-02 - - 2.4390244483947754e-01 -6.2283736467361450e-01 - <_> - - 0 -1 75 1.2220342177897692e-03 - - -3.5440090298652649e-01 3.2636523246765137e-01 - <_> - - 0 -1 86 -1.7682027537375689e-03 - - -8.3522576093673706e-01 1.3612115383148193e-01 - <_> - - 0 -1 46 2.1033132448792458e-02 - - -6.1646652221679688e-01 2.0369003713130951e-01 - <_> - - 0 -1 89 4.2734121961984783e-05 - - -3.1042623519897461e-01 3.7497150897979736e-01 - <_> - - 0 -1 121 2.0566717721521854e-03 - - 1.3792866468429565e-01 -8.7232136726379395e-01 - <_> - - 0 -1 68 -2.6542316190898418e-03 - - -6.4538162946701050e-01 1.3908456265926361e-01 - <_> - - 0 -1 20 3.9702588692307472e-03 - - -7.7219176292419434e-01 1.2768661975860596e-01 - <_> - - 0 -1 83 -2.1511501108761877e-04 - - 4.2976233363151550e-01 -3.0495703220367432e-01 - <_> - - 0 -1 61 1.1118821799755096e-02 - - 1.6775466501712799e-01 -8.0201923847198486e-01 - <_> - - 0 -1 108 -1.0669567927834578e-05 - - 1.0882959514856339e-01 -7.9875063896179199e-01 - <_> - - 0 -1 116 -3.4705772995948792e-03 - - -9.0627700090408325e-01 8.9696519076824188e-02 - - <_> - 11 - -1.3003323078155518e+00 - - <_> - - 0 -1 53 -3.8354307413101196e-02 - - 1.2666666507720947e-01 -7.0392155647277832e-01 - <_> - - 0 -1 98 -9.7261724295094609e-04 - - 2.8604879975318909e-01 -4.1622883081436157e-01 - <_> - - 0 -1 3 2.4279407225549221e-03 - - -6.0584521293640137e-01 1.9692152738571167e-01 - <_> - - 0 -1 49 -2.0870115607976913e-02 - - -7.8636735677719116e-01 2.0237915217876434e-01 - <_> - - 0 -1 111 5.5331725161522627e-04 - - -2.9303926229476929e-01 5.3937625885009766e-01 - <_> - - 0 -1 82 -1.1471309699118137e-03 - - 4.4445949792861938e-01 -2.6888713240623474e-01 - <_> - - 0 -1 95 -5.8944348711520433e-04 - - 4.8110336065292358e-01 -2.7363428473472595e-01 - <_> - - 0 -1 32 4.0494561195373535e-02 - - -2.2043995559215546e-01 5.2403515577316284e-01 - <_> - - 0 -1 28 1.5283776447176933e-03 - - 1.8056891858577728e-01 -6.9702959060668945e-01 - <_> - - 0 -1 92 -1.5782285481691360e-04 - - 4.5709878206253052e-01 -2.5625130534172058e-01 - <_> - - 0 -1 81 -1.6659669578075409e-02 - - -8.5107403993606567e-01 1.2532263994216919e-01 - - <_> - - <_> - 0 0 2 20 -1. - <_> - 0 10 2 10 2. - 0 - <_> - - <_> - 0 0 2 24 -1. - <_> - 0 12 2 12 2. - 0 - <_> - - <_> - 0 0 6 7 -1. - <_> - 3 0 3 7 2. - 0 - <_> - - <_> - 0 0 10 7 -1. - <_> - 5 0 5 7 2. - 0 - <_> - - <_> - 0 0 10 20 -1. - <_> - 0 10 10 10 2. - 0 - <_> - - <_> - 0 0 24 1 -1. - <_> - 12 0 12 1 2. - 0 - <_> - - <_> - 0 1 24 4 -1. - <_> - 8 1 8 4 3. - 0 - <_> - - <_> - 0 1 22 6 -1. - <_> - 0 3 22 2 3. - 0 - <_> - - <_> - 0 2 1 22 -1. - <_> - 0 13 1 11 2. - 0 - <_> - - <_> - 0 2 16 6 -1. - <_> - 8 2 8 6 2. - 0 - <_> - - <_> - 0 2 21 3 -1. - <_> - 0 3 21 1 3. - 0 - <_> - - <_> - 0 3 1 18 -1. - <_> - 0 9 1 6 3. - 0 - <_> - - <_> - 0 3 24 17 -1. - <_> - 8 3 8 17 3. - 0 - <_> - - <_> - 0 3 24 18 -1. - <_> - 8 3 8 18 3. - 0 - <_> - - <_> - 0 7 10 13 -1. - <_> - 5 7 5 13 2. - 0 - <_> - - <_> - 0 8 24 13 -1. - <_> - 8 8 8 13 3. - 0 - <_> - - <_> - 0 8 19 2 -1. - <_> - 0 9 19 1 2. - 0 - <_> - - <_> - 0 9 6 1 -1. - <_> - 3 9 3 1 2. - 0 - <_> - - <_> - 0 9 9 4 -1. - <_> - 0 11 9 2 2. - 0 - <_> - - <_> - 0 10 22 14 -1. - <_> - 0 17 22 7 2. - 0 - <_> - - <_> - 0 11 8 11 -1. - <_> - 4 11 4 11 2. - 0 - <_> - - <_> - 0 12 14 1 -1. - <_> - 7 12 7 1 2. - 0 - <_> - - <_> - 0 12 14 9 -1. - <_> - 7 12 7 9 2. - 0 - <_> - - <_> - 0 12 23 12 -1. - <_> - 0 18 23 6 2. - 0 - <_> - - <_> - 0 14 6 9 -1. - <_> - 3 14 3 9 2. - 0 - <_> - - <_> - 0 14 24 6 -1. - <_> - 8 14 8 6 3. - 0 - <_> - - <_> - 0 17 10 1 -1. - <_> - 5 17 5 1 2. - 0 - <_> - - <_> - 0 18 10 6 -1. - <_> - 0 21 10 3 2. - 0 - <_> - - <_> - 0 22 24 1 -1. - <_> - 12 22 12 1 2. - 0 - <_> - - <_> - 0 23 18 1 -1. - <_> - 9 23 9 1 2. - 0 - <_> - - <_> - 0 23 22 1 -1. - <_> - 11 23 11 1 2. - 0 - <_> - - <_> - 1 0 16 5 -1. - <_> - 9 0 8 5 2. - 0 - <_> - - <_> - 1 3 20 6 -1. - <_> - 1 3 10 3 2. - <_> - 11 6 10 3 2. - 0 - <_> - - <_> - 1 4 8 1 -1. - <_> - 5 4 4 1 2. - 0 - <_> - - <_> - 1 7 14 6 -1. - <_> - 1 10 14 3 2. - 0 - <_> - - <_> - 1 8 4 3 -1. - <_> - 3 8 2 3 2. - 0 - <_> - - <_> - 1 8 10 6 -1. - <_> - 1 11 10 3 2. - 0 - <_> - - <_> - 1 23 16 1 -1. - <_> - 9 23 8 1 2. - 0 - <_> - - <_> - 2 0 22 2 -1. - <_> - 13 0 11 2 2. - 0 - <_> - - <_> - 2 0 22 4 -1. - <_> - 13 0 11 4 2. - 0 - <_> - - <_> - 2 0 13 4 -1. - <_> - 2 2 13 2 2. - 0 - <_> - - <_> - 2 5 18 14 -1. - <_> - 11 5 9 14 2. - 0 - <_> - - <_> - 2 6 8 4 -1. - <_> - 2 8 8 2 2. - 0 - <_> - - <_> - 2 14 20 10 -1. - <_> - 2 19 20 5 2. - 0 - <_> - - <_> - 2 19 22 2 -1. - <_> - 2 20 22 1 2. - 0 - <_> - - <_> - 2 20 16 4 -1. - <_> - 10 20 8 4 2. - 0 - <_> - - <_> - 3 0 21 7 -1. - <_> - 10 0 7 7 3. - 0 - <_> - - <_> - 3 0 17 10 -1. - <_> - 3 5 17 5 2. - 0 - <_> - - <_> - 3 0 19 10 -1. - <_> - 3 5 19 5 2. - 0 - <_> - - <_> - 3 6 15 8 -1. - <_> - 3 10 15 4 2. - 0 - <_> - - <_> - 3 20 8 4 -1. - <_> - 3 22 8 2 2. - 0 - <_> - - <_> - 4 0 20 2 -1. - <_> - 14 0 10 2 2. - 0 - <_> - - <_> - 4 7 16 8 -1. - <_> - 12 7 8 8 2. - 0 - <_> - - <_> - 4 11 20 8 -1. - <_> - 4 11 10 4 2. - <_> - 14 15 10 4 2. - 0 - <_> - - <_> - 4 13 4 4 -1. - <_> - 4 13 2 2 2. - <_> - 6 15 2 2 2. - 0 - <_> - - <_> - 5 0 2 8 -1. - <_> - 5 4 2 4 2. - 0 - <_> - - <_> - 5 0 11 4 -1. - <_> - 5 2 11 2 2. - 0 - <_> - - <_> - 5 0 17 8 -1. - <_> - 5 4 17 4 2. - 0 - <_> - - <_> - 5 0 18 6 -1. - <_> - 5 2 18 2 3. - 0 - <_> - - <_> - 5 0 19 18 -1. - <_> - 5 9 19 9 2. - 0 - <_> - - <_> - 5 5 3 4 -1. - <_> - 5 7 3 2 2. - 0 - <_> - - <_> - 5 8 6 6 -1. - <_> - 5 11 6 3 2. - 0 - <_> - - <_> - 5 8 14 9 -1. - <_> - 5 11 14 3 3. - 0 - <_> - - <_> - 5 9 13 15 -1. - <_> - 5 14 13 5 3. - 0 - <_> - - <_> - 5 14 15 10 -1. - <_> - 5 19 15 5 2. - 0 - <_> - - <_> - 5 14 17 10 -1. - <_> - 5 19 17 5 2. - 0 - <_> - - <_> - 6 12 11 12 -1. - <_> - 6 16 11 4 3. - 0 - <_> - - <_> - 6 16 12 8 -1. - <_> - 6 20 12 4 2. - 0 - <_> - - <_> - 6 22 18 2 -1. - <_> - 15 22 9 2 2. - 0 - <_> - - <_> - 6 23 18 1 -1. - <_> - 15 23 9 1 2. - 0 - <_> - - <_> - 7 14 13 10 -1. - <_> - 7 19 13 5 2. - 0 - <_> - - <_> - 7 16 8 8 -1. - <_> - 7 20 8 4 2. - 0 - <_> - - <_> - 8 0 1 20 -1. - <_> - 8 10 1 10 2. - 0 - <_> - - <_> - 8 0 2 17 -1. - <_> - 9 0 1 17 2. - 0 - <_> - - <_> - 8 7 14 14 -1. - <_> - 8 7 7 7 2. - <_> - 15 14 7 7 2. - 0 - <_> - - <_> - 8 13 1 6 -1. - <_> - 8 15 1 2 3. - 0 - <_> - - <_> - 8 20 9 3 -1. - <_> - 8 21 9 1 3. - 0 - <_> - - <_> - 8 21 16 3 -1. - <_> - 16 21 8 3 2. - 0 - <_> - - <_> - 9 9 1 4 -1. - <_> - 9 11 1 2 2. - 0 - <_> - - <_> - 10 1 14 4 -1. - <_> - 10 1 7 2 2. - <_> - 17 3 7 2 2. - 0 - <_> - - <_> - 10 14 11 10 -1. - <_> - 10 19 11 5 2. - 0 - <_> - - <_> - 10 15 9 8 -1. - <_> - 13 15 3 8 3. - 0 - <_> - - <_> - 11 1 10 3 -1. - <_> - 11 2 10 1 3. - 0 - <_> - - <_> - 11 3 3 3 -1. - <_> - 12 3 1 3 3. - 0 - <_> - - <_> - 11 4 4 9 -1. - <_> - 11 7 4 3 3. - 0 - <_> - - <_> - 11 9 8 10 -1. - <_> - 11 9 4 5 2. - <_> - 15 14 4 5 2. - 0 - <_> - - <_> - 12 0 12 1 -1. - <_> - 18 0 6 1 2. - 0 - <_> - - <_> - 12 1 12 7 -1. - <_> - 18 1 6 7 2. - 0 - <_> - - <_> - 12 9 7 4 -1. - <_> - 12 11 7 2 2. - 0 - <_> - - <_> - 12 12 1 2 -1. - <_> - 12 13 1 1 2. - 0 - <_> - - <_> - 12 22 12 2 -1. - <_> - 18 22 6 2 2. - 0 - <_> - - <_> - 13 0 8 24 -1. - <_> - 13 8 8 8 3. - 0 - <_> - - <_> - 13 4 3 5 -1. - <_> - 14 4 1 5 3. - 0 - <_> - - <_> - 13 8 5 6 -1. - <_> - 13 10 5 2 3. - 0 - <_> - - <_> - 13 9 10 12 -1. - <_> - 13 9 5 6 2. - <_> - 18 15 5 6 2. - 0 - <_> - - <_> - 13 14 8 2 -1. - <_> - 13 14 4 1 2. - <_> - 17 15 4 1 2. - 0 - <_> - - <_> - 13 15 10 2 -1. - <_> - 13 15 5 1 2. - <_> - 18 16 5 1 2. - 0 - <_> - - <_> - 14 1 10 6 -1. - <_> - 14 1 5 3 2. - <_> - 19 4 5 3 2. - 0 - <_> - - <_> - 14 8 8 9 -1. - <_> - 14 11 8 3 3. - 0 - <_> - - <_> - 15 16 9 4 -1. - <_> - 15 18 9 2 2. - 0 - <_> - - <_> - 15 18 9 6 -1. - <_> - 15 21 9 3 2. - 0 - <_> - - <_> - 15 22 8 2 -1. - <_> - 15 23 8 1 2. - 0 - <_> - - <_> - 16 4 8 13 -1. - <_> - 20 4 4 13 2. - 0 - <_> - - <_> - 16 4 8 16 -1. - <_> - 20 4 4 16 2. - 0 - <_> - - <_> - 16 5 8 14 -1. - <_> - 20 5 4 14 2. - 0 - <_> - - <_> - 16 6 8 15 -1. - <_> - 20 6 4 15 2. - 0 - <_> - - <_> - 16 14 8 4 -1. - <_> - 20 14 4 4 2. - 0 - <_> - - <_> - 17 12 6 1 -1. - <_> - 20 12 3 1 2. - 0 - <_> - - <_> - 17 17 5 6 -1. - <_> - 17 20 5 3 2. - 0 - <_> - - <_> - 18 5 6 6 -1. - <_> - 21 5 3 6 2. - 0 - <_> - - <_> - 18 7 6 7 -1. - <_> - 21 7 3 7 2. - 0 - <_> - - <_> - 18 15 2 4 -1. - <_> - 18 15 1 2 2. - <_> - 19 17 1 2 2. - 0 - <_> - - <_> - 20 8 4 4 -1. - <_> - 22 8 2 4 2. - 0 - <_> - - <_> - 20 8 4 16 -1. - <_> - 20 16 4 8 2. - 0 - <_> - - <_> - 21 0 3 24 -1. - <_> - 21 12 3 12 2. - 0 - <_> - - <_> - 21 1 3 18 -1. - <_> - 21 7 3 6 3. - 0 - <_> - - <_> - 22 0 2 20 -1. - <_> - 22 10 2 10 2. - 0 - <_> - - <_> - 22 13 2 10 -1. - <_> - 22 18 2 5 2. - 0 - <_> - - <_> - 23 0 1 12 -1. - <_> - 23 6 1 6 2. - 0 - <_> - - <_> - 23 0 1 20 -1. - <_> - 23 10 1 10 2. - 0 - <_> - - <_> - 23 0 1 24 -1. - <_> - 23 12 1 12 2. - 0 - <_> - - <_> - 23 2 1 22 -1. - <_> - 23 13 1 11 2. - 0 - diff --git a/moon/xml/homebase.xml b/moon/xml/homebase.xml deleted file mode 100644 index c3ce3eb7a..000000000 --- a/moon/xml/homebase.xml +++ /dev/null @@ -1,792 +0,0 @@ - - - - BOOST - HAAR - 24 - 24 - - GAB - 9.9500000476837158e-01 - 5.0000000000000000e-01 - 9.4999999999999996e-01 - 1 - 100 - - 0 - 1 - BASIC - 13 - - - <_> - 1 - 9.7005987167358398e-01 - - <_> - - 0 -1 2 1.9361749291419983e-01 - - -9.9799197912216187e-01 9.7005987167358398e-01 - - <_> - 3 - -1.1428333520889282e+00 - - <_> - - 0 -1 26 1.0809595882892609e-01 - - -9.5740365982055664e-01 7.9651165008544922e-01 - <_> - - 0 -1 45 -5.3553197532892227e-02 - - 7.5217384099960327e-01 -8.2870697975158691e-01 - <_> - - 0 -1 28 5.8048516511917114e-02 - - -8.5232347249984741e-01 6.4327728748321533e-01 - - <_> - 3 - -1.1155120134353638e+00 - - <_> - - 0 -1 7 5.3093338012695312e-01 - - -8.4629803895950317e-01 8.8593155145645142e-01 - <_> - - 0 -1 22 9.4513632357120514e-02 - - -7.5412440299987793e-01 6.7376071214675903e-01 - <_> - - 0 -1 32 1.8800559337250888e-04 - - -9.4297468662261963e-01 5.0391024351119995e-01 - - <_> - 3 - -1.1647795438766479e+00 - - <_> - - 0 -1 10 5.2520900964736938e-01 - - -8.4295177459716797e-01 8.0952382087707520e-01 - <_> - - 0 -1 14 -4.7649919986724854e-02 - - 5.3026688098907471e-01 -7.9501885175704956e-01 - <_> - - 0 -1 17 2.8573157032951713e-04 - - -9.6133655309677124e-01 4.7319105267524719e-01 - - <_> - 5 - -1.6264814138412476e+00 - - <_> - - 0 -1 29 -5.9020552784204483e-02 - - 5.2777779102325439e-01 -8.8659793138504028e-01 - <_> - - 0 -1 1 3.3631715178489685e-01 - - -8.3214259147644043e-01 4.6986281871795654e-01 - <_> - - 0 -1 53 -6.7099377512931824e-02 - - 3.9218667149543762e-01 -9.0403634309768677e-01 - <_> - - 0 -1 42 1.1503914371132851e-02 - - -6.4935868978500366e-01 6.0701298713684082e-01 - <_> - - 0 -1 52 5.4596010595560074e-02 - - -9.8449242115020752e-01 3.4943118691444397e-01 - - <_> - 5 - -1.6929202079772949e+00 - - <_> - - 0 -1 4 6.8405115604400635e-01 - - -7.9277569055557251e-01 5.8992803096771240e-01 - <_> - - 0 -1 39 -5.6607984006404877e-03 - - 4.8237881064414978e-01 -7.7428668737411499e-01 - <_> - - 0 -1 5 -1.5654803064535372e-05 - - 4.8617321252822876e-01 -6.8095266819000244e-01 - <_> - - 0 -1 18 3.6388423177413642e-04 - - -9.3085920810699463e-01 3.1755030155181885e-01 - <_> - - 0 -1 55 1.0680839419364929e-02 - - -9.5543885231018066e-01 2.3754456639289856e-01 - - <_> - 5 - -1.4901319742202759e+00 - - <_> - - 0 -1 43 7.0012055337429047e-02 - - -8.6243385076522827e-01 3.7662336230278015e-01 - <_> - - 0 -1 38 7.8976482152938843e-02 - - -7.5742954015731812e-01 4.7752013802528381e-01 - <_> - - 0 -1 16 4.9195237457752228e-02 - - -7.4817359447479248e-01 3.9996236562728882e-01 - <_> - - 0 -1 23 -9.5423578750342131e-04 - - 4.7555682063102722e-01 -6.4422392845153809e-01 - <_> - - 0 -1 46 -1.0019879846367985e-04 - - 4.0234825015068054e-01 -7.2886103391647339e-01 - - <_> - 5 - -1.4097670316696167e+00 - - <_> - - 0 -1 49 -3.1540390104055405e-02 - - 5.3076922893524170e-01 -7.5514018535614014e-01 - <_> - - 0 -1 51 -5.5787548422813416e-02 - - 4.5242163538932800e-01 -7.9780071973800659e-01 - <_> - - 0 -1 33 -3.4717231988906860e-02 - - 5.7161980867385864e-01 -5.5604815483093262e-01 - <_> - - 0 -1 20 3.2852803997229785e-05 - - -7.1000611782073975e-01 3.6933586001396179e-01 - <_> - - 0 -1 24 -8.0002757022157311e-04 - - 3.2988607883453369e-01 -8.2379430532455444e-01 - - <_> - 6 - -1.9609961509704590e+00 - - <_> - - 0 -1 27 -4.3925424688495696e-04 - - 2.2939866781234741e-01 -8.7741202116012573e-01 - <_> - - 0 -1 11 2.3729677498340607e-01 - - -6.1719685792922974e-01 6.6190558671951294e-01 - <_> - - 0 -1 56 1.3697046961169690e-04 - - -9.7690749168395996e-01 2.4668875336647034e-01 - <_> - - 0 -1 41 4.0579132735729218e-02 - - -4.6990355849266052e-01 5.9613120555877686e-01 - <_> - - 0 -1 31 1.4998754858970642e-01 - - -6.1468529701232910e-01 4.5909219980239868e-01 - <_> - - 0 -1 54 -7.6336357742547989e-03 - - 3.7151280045509338e-01 -9.7677791118621826e-01 - - <_> - 6 - -1.2625006437301636e+00 - - <_> - - 0 -1 19 1.3542500138282776e-01 - - -7.7492594718933105e-01 3.6277601122856140e-01 - <_> - - 0 -1 13 -1.6061423346400261e-02 - - 3.0234134197235107e-01 -8.4298843145370483e-01 - <_> - - 0 -1 57 2.2906231606611982e-05 - - -7.9203683137893677e-01 2.5057432055473328e-01 - <_> - - 0 -1 6 1.5224516391754150e-03 - - -8.0667692422866821e-01 3.0236306786537170e-01 - <_> - - 0 -1 34 6.0298316180706024e-02 - - -6.3095331192016602e-01 5.0628435611724854e-01 - <_> - - 0 -1 40 2.8275467921048403e-03 - - -5.9185290336608887e-01 4.3342962861061096e-01 - - <_> - 6 - -1.8548942804336548e+00 - - <_> - - 0 -1 12 5.7576465606689453e-01 - - -7.5901329517364502e-01 4.7101449966430664e-01 - <_> - - 0 -1 25 1.1199696455150843e-03 - - -8.2158964872360229e-01 2.7694737911224365e-01 - <_> - - 0 -1 37 -1.3452430721372366e-03 - - 3.8042023777961731e-01 -5.8596962690353394e-01 - <_> - - 0 -1 30 -4.7689385712146759e-02 - - 2.9166099429130554e-01 -7.7451306581497192e-01 - <_> - - 0 -1 0 4.5107162441127002e-04 - - 2.9008096456527710e-01 -7.4628704786300659e-01 - <_> - - 0 -1 44 4.6792356297373772e-03 - - -4.3391737341880798e-01 6.1101162433624268e-01 - - <_> - 5 - -1.4520095586776733e+00 - - <_> - - 0 -1 50 2.0277120638638735e-03 - - -7.4658870697021484e-01 3.1578946113586426e-01 - <_> - - 0 -1 9 1.9998335838317871e-01 - - -6.0470157861709595e-01 4.4546359777450562e-01 - <_> - - 0 -1 48 -2.2651432082056999e-03 - - 5.0624489784240723e-01 -7.1044588088989258e-01 - <_> - - 0 -1 21 8.1743783084675670e-05 - - -9.6728926897048950e-01 2.7392283082008362e-01 - <_> - - 0 -1 47 -6.2081694602966309e-02 - - 3.3580368757247925e-01 -6.8506646156311035e-01 - - <_> - 6 - -1.3946850299835205e+00 - - <_> - - 0 -1 15 3.0213516205549240e-02 - - -7.3080569505691528e-01 3.6727273464202881e-01 - <_> - - 0 -1 35 2.1714199334383011e-02 - - -7.7477943897247314e-01 3.7046548724174500e-01 - <_> - - 0 -1 36 3.8260325789451599e-02 - - -7.1073615550994873e-01 3.2503500580787659e-01 - <_> - - 0 -1 53 -6.9243669509887695e-02 - - 3.3552998304367065e-01 -7.9664427042007446e-01 - <_> - - 0 -1 8 -1.0030823759734631e-03 - - 3.2309907674789429e-01 -8.1674295663833618e-01 - <_> - - 0 -1 3 -7.6404446735978127e-04 - - -9.8253381252288818e-01 2.5941035151481628e-01 - - <_> - - <_> - 0 0 1 10 -1. - <_> - 0 5 1 5 2. - 0 - <_> - - <_> - 0 0 21 18 -1. - <_> - 7 0 7 18 3. - 0 - <_> - - <_> - 0 0 24 17 -1. - <_> - 8 0 8 17 3. - 0 - <_> - - <_> - 0 1 3 6 -1. - <_> - 0 4 3 3 2. - 0 - <_> - - <_> - 0 1 24 19 -1. - <_> - 8 1 8 19 3. - 0 - <_> - - <_> - 0 3 23 2 -1. - <_> - 0 4 23 1 2. - 0 - <_> - - <_> - 0 4 16 1 -1. - <_> - 8 4 8 1 2. - 0 - <_> - - <_> - 0 5 24 13 -1. - <_> - 8 5 8 13 3. - 0 - <_> - - <_> - 0 6 5 18 -1. - <_> - 0 12 5 6 3. - 0 - <_> - - <_> - 0 6 12 18 -1. - <_> - 6 6 6 18 2. - 0 - <_> - - <_> - 0 6 24 12 -1. - <_> - 8 6 8 12 3. - 0 - <_> - - <_> - 0 7 24 4 -1. - <_> - 8 7 8 4 3. - 0 - <_> - - <_> - 0 7 24 11 -1. - <_> - 8 7 8 11 3. - 0 - <_> - - <_> - 0 9 13 9 -1. - <_> - 0 12 13 3 3. - 0 - <_> - - <_> - 0 9 23 9 -1. - <_> - 0 12 23 3 3. - 0 - <_> - - <_> - 0 10 14 1 -1. - <_> - 7 10 7 1 2. - 0 - <_> - - <_> - 0 11 12 5 -1. - <_> - 6 11 6 5 2. - 0 - <_> - - <_> - 0 14 1 10 -1. - <_> - 0 19 1 5 2. - 0 - <_> - - <_> - 1 5 16 1 -1. - <_> - 9 5 8 1 2. - 0 - <_> - - <_> - 2 15 21 3 -1. - <_> - 9 15 7 3 3. - 0 - <_> - - <_> - 3 0 12 1 -1. - <_> - 9 0 6 1 2. - 0 - <_> - - <_> - 4 1 1 16 -1. - <_> - 4 9 1 8 2. - 0 - <_> - - <_> - 5 4 14 6 -1. - <_> - 5 7 14 3 2. - 0 - <_> - - <_> - 5 11 18 2 -1. - <_> - 5 12 18 1 2. - 0 - <_> - - <_> - 5 18 16 6 -1. - <_> - 5 21 16 3 2. - 0 - <_> - - <_> - 6 0 16 2 -1. - <_> - 6 1 16 1 2. - 0 - <_> - - <_> - 6 3 14 8 -1. - <_> - 6 7 14 4 2. - 0 - <_> - - <_> - 6 3 15 2 -1. - <_> - 6 4 15 1 2. - 0 - <_> - - <_> - 6 4 12 6 -1. - <_> - 6 7 12 3 2. - 0 - <_> - - <_> - 6 9 13 9 -1. - <_> - 6 12 13 3 3. - 0 - <_> - - <_> - 7 0 9 9 -1. - <_> - 7 3 9 3 3. - 0 - <_> - - <_> - 7 3 13 12 -1. - <_> - 7 7 13 4 3. - 0 - <_> - - <_> - 8 0 10 2 -1. - <_> - 8 1 10 1 2. - 0 - <_> - - <_> - 8 2 7 6 -1. - <_> - 8 4 7 2 3. - 0 - <_> - - <_> - 8 3 5 8 -1. - <_> - 8 7 5 4 2. - 0 - <_> - - <_> - 8 12 9 6 -1. - <_> - 8 15 9 3 2. - 0 - <_> - - <_> - 9 4 4 9 -1. - <_> - 9 7 4 3 3. - 0 - <_> - - <_> - 9 4 15 1 -1. - <_> - 14 4 5 1 3. - 0 - <_> - - <_> - 9 4 8 9 -1. - <_> - 9 7 8 3 3. - 0 - <_> - - <_> - 9 8 3 6 -1. - <_> - 9 11 3 3 2. - 0 - <_> - - <_> - 9 10 4 5 -1. - <_> - 11 10 2 5 2. - 0 - <_> - - <_> - 9 12 5 9 -1. - <_> - 9 15 5 3 3. - 0 - <_> - - <_> - 9 12 6 6 -1. - <_> - 9 15 6 3 2. - 0 - <_> - - <_> - 9 12 11 12 -1. - <_> - 9 16 11 4 3. - 0 - <_> - - <_> - 9 13 7 2 -1. - <_> - 9 14 7 1 2. - 0 - <_> - - <_> - 9 15 6 8 -1. - <_> - 9 19 6 4 2. - 0 - <_> - - <_> - 10 0 12 2 -1. - <_> - 10 0 6 1 2. - <_> - 16 1 6 1 2. - 0 - <_> - - <_> - 10 2 14 7 -1. - <_> - 17 2 7 7 2. - 0 - <_> - - <_> - 10 3 14 1 -1. - <_> - 17 3 7 1 2. - 0 - <_> - - <_> - 10 11 4 6 -1. - <_> - 10 13 4 2 3. - 0 - <_> - - <_> - 11 11 2 3 -1. - <_> - 11 12 2 1 3. - 0 - <_> - - <_> - 12 13 12 5 -1. - <_> - 18 13 6 5 2. - 0 - <_> - - <_> - 13 0 11 12 -1. - <_> - 13 6 11 6 2. - 0 - <_> - - <_> - 14 9 10 11 -1. - <_> - 19 9 5 11 2. - 0 - <_> - - <_> - 16 0 8 18 -1. - <_> - 16 0 4 9 2. - <_> - 20 9 4 9 2. - 0 - <_> - - <_> - 16 0 4 22 -1. - <_> - 16 11 4 11 2. - 0 - <_> - - <_> - 21 0 1 20 -1. - <_> - 21 10 1 10 2. - 0 - <_> - - <_> - 23 15 1 8 -1. - <_> - 23 19 1 4 2. - 0 -