From f1c4402be180b2eac112471e0c9da49b78ebe715 Mon Sep 17 00:00:00 2001 From: Carter Frost Date: Wed, 31 Mar 2021 20:58:02 -0700 Subject: [PATCH 01/18] src/scoot/src/Scoot.py: sync lock decorator rewrite, +drive sync lock --- src/scoot/src/Scoot.py | 21 ++++++++++++--------- 1 file changed, 12 insertions(+), 9 deletions(-) diff --git a/src/scoot/src/Scoot.py b/src/scoot/src/Scoot.py index 725af5c..bf4d514 100755 --- a/src/scoot/src/Scoot.py +++ b/src/scoot/src/Scoot.py @@ -20,6 +20,7 @@ from functools import wraps odom_lock = threading.Lock() +drive_lock = threading.Lock() class DriveException(Exception): @@ -79,14 +80,15 @@ class TimeoutException(DriveException): pass -# def sync(lock): -# def _sync(func): -# @wraps(func) -# def wrapper(*args, **kwargs): -# with lock: -# return func(*args, **kwargs) -# return wrapper -# return _sync +class sync(object): + def __init__(self, lock): + self.lock = lock + def __call__(self, func): + def wrapped_f(*args,**kwargs): + with self.lock: + return func(*args,**kwargs) + return wrapped_f + class Location: """A class that encodes an EKF provided location and accessor methods""" @@ -250,7 +252,7 @@ def start(self, **kwargs): self.xform = tf.TransformListener() rospy.loginfo("Scoot Ready") - # @sync(odom_lock) + @sync(odom_lock) def _odom(self, msg): self.OdomLocation.Odometry = msg @@ -412,6 +414,7 @@ def set_heading(self, heading, **kwargs): angle = angles.shortest_angular_distance(loc.theta, heading) self.turn(angle, **kwargs) + @sync(drive_lock) def __drive(self, request, **kwargs): request.obstacles = ~0 if 'ignore' in kwargs: From b2f1d09379227d166fa3c64e8adac31e9e64d0fd Mon Sep 17 00:00:00 2001 From: Carter Frost Date: Mon, 5 Apr 2021 01:10:14 -0700 Subject: [PATCH 02/18] src/scoot/src/Scoot.py: added function locks and some PEP8 formatting --- src/scoot/src/Scoot.py | 112 +++++++++++++++++++++++------------------ 1 file changed, 64 insertions(+), 48 deletions(-) diff --git a/src/scoot/src/Scoot.py b/src/scoot/src/Scoot.py index bf4d514..26dd7ec 100755 --- a/src/scoot/src/Scoot.py +++ b/src/scoot/src/Scoot.py @@ -21,6 +21,7 @@ odom_lock = threading.Lock() drive_lock = threading.Lock() +joint_lock = threading.Lock() class DriveException(Exception): @@ -81,13 +82,15 @@ class TimeoutException(DriveException): class sync(object): - def __init__(self, lock): - self.lock = lock - def __call__(self, func): - def wrapped_f(*args,**kwargs): - with self.lock: - return func(*args,**kwargs) - return wrapped_f + def __init__(self, lock): + self.lock = lock + + def __call__(self, func): + def wrapped_f(*args, **kwargs): + with self.lock: + return func(*args, **kwargs) + + return wrapped_f class Location: @@ -97,12 +100,12 @@ def __init__(self, odo): self.Odometry = odo def getPose(self): # TODO: add a ros warning if self.Odometry none - '''Return a std_msgs.Pose from this Location. Useful because Pose + '''Return a std_msgs.Pose from this Location. Useful because Pose has angles represented as roll, pitch, yaw. - + Returns: - - * (`std_msgs.msg.Pose`) The pose. + + * (`std_msgs.msg.Pose`) The pose. ''' if self.Odometry is None: quat = [0, 0, 0, 0, ] @@ -195,9 +198,9 @@ def start(self, **kwargs): self.rover_name = rospy.get_namespace() self.rover_name = self.rover_name.strip('/') self.rover_type = self.rover_name.split("_")[1] # cuts of the _# part of the rover name - self.DRIVE_SPEED = rospy.get_param("/"+self.rover_name+"/Core/DRIVE_SPEED", default=5) - self.REVERSE_SPEED = rospy.get_param("/"+self.rover_name+"/Core/REVERSE_SPEED", default=5) - self.TURN_SPEED = rospy.get_param("/"+self.rover_name+"/Core/TURN_SPEED", default=5) + self.DRIVE_SPEED = rospy.get_param("/" + self.rover_name + "/Core/DRIVE_SPEED", default=5) + self.REVERSE_SPEED = rospy.get_param("/" + self.rover_name + "/Core/REVERSE_SPEED", default=5) + self.TURN_SPEED = rospy.get_param("/" + self.rover_name + "/Core/TURN_SPEED", default=5) '''Tracking SRCP2's Wiki Documentation/API/Robots/Hauler.md @@ -207,13 +210,16 @@ def start(self, **kwargs): our scoot.launch and matching with indexes from our Obstacles.msg ''' self.VOL_TYPES = rospy.get_param("vol_types", - default=["ice", "ethene", "methane", "methanol", "carbon_dioxide", "ammonia", "hydrogen_sulfite", "sulfur_dioxide", "regolith"]) + default=["ice", "ethene", "methane", "methanol", "carbon_dioxide", "ammonia", + "hydrogen_sulfite", "sulfur_dioxide", "regolith"]) # @NOTE: when we use namespaces we wont need to have the rover_name # Create publishers. - #self.skid_topic = rospy.Publisher('/' + self.rover_name + '/skid_cmd_vel', Twist, queue_size=10) - self.sensor_pitch_control_topic = rospy.Publisher('/' + self.rover_name + '/sensor/pitch/command/position', Float64, queue_size=10) - self.sensor_yaw_control_topic = rospy.Publisher('/' + self.rover_name + '/sensor/yaw/command/position', Float64, queue_size=10) + # self.skid_topic = rospy.Publisher('/' + self.rover_name + '/skid_cmd_vel', Twist, queue_size=10) + self.sensor_pitch_control_topic = rospy.Publisher('/' + self.rover_name + '/sensor/pitch/command/position', + Float64, queue_size=10) + self.sensor_yaw_control_topic = rospy.Publisher('/' + self.rover_name + '/sensor/yaw/command/position', Float64, + queue_size=10) # Connect to services. rospy.loginfo("Waiting for control service") rospy.wait_for_service('/' + self.rover_name + '/control') @@ -226,27 +232,29 @@ def start(self, **kwargs): if self.rover_type == "scout": pass # rospy.wait_for_service('/vol_detected_service') elif self.rover_type == "excavator": - self.shoulder_yaw_control = rospy.Publisher('/' + self.rover_name + '/arm/shoulder_yaw/command/position', Float64, - queue_size=10) - self.shoulder_pitch_control = rospy.Publisher('/' + self.rover_name + '/arm/shoulder_pitch/command/position', - Float64, queue_size=10) + self.shoulder_yaw_control = rospy.Publisher('/' + self.rover_name + '/arm/shoulder_yaw/command/position', + Float64, + queue_size=10) + self.shoulder_pitch_control = rospy.Publisher( + '/' + self.rover_name + '/arm/shoulder_pitch/command/position', + Float64, queue_size=10) self.bucket_control = rospy.Publisher('/' + self.rover_name + '/arm/wrist_pitch/command/position', Float64, queue_size=10) self.elbow_pitch_control = rospy.Publisher('/' + self.rover_name + '/arm/elbow_pitch/command/position', - Float64, queue_size=10) - + Float64, queue_size=10) + rospy.Subscriber('/' + self.rover_name + '/scoop_info', msg.ExcavatorScoopMsg, self._bucket_info) elif self.rover_type == "hauler": self.bin_control = rospy.Publisher('/' + self.rover_name + '/bin/command/position', Float64, queue_size=10) - #rospy.Subscriber('/' + self.rover_name + '/bin_info', msg.HaulerMsg, self._bin_info) + # rospy.Subscriber('/' + self.rover_name + '/bin_info', msg.HaulerMsg, self._bin_info) rospy.loginfo("Done waiting for rover specific services") # Subscribe to topics. rospy.Subscriber('/' + self.rover_name + '/odometry/filtered', Odometry, self._odom) rospy.Subscriber('/' + self.rover_name + '/joint_states', JointState, self._joint_states) - rospy.Subscriber('/srcp2/score', msg.ScoreMsg , self._score) + rospy.Subscriber('/srcp2/score', msg.ScoreMsg, self._score) # Transform listener. Use this to transform between coordinate spaces. # Transform messages must predate any sensor messages so initialize this first. self.xform = tf.TransformListener() @@ -277,10 +285,12 @@ def get_joint_pos(self, joint_name): rospy.logerr("get_joint_state: unknown joint:" + str(joint_name)) rospy.loginfo("get_joint_state: valid joints" + str(self.joint_states.name)) + @sync(odom_lock) def getOdomLocation(self): with odom_lock: return self.OdomLocation + @sync(odom_lock) def getTruePose(self): if self.truePoseCalled: print("True pose already called once.") @@ -494,15 +504,16 @@ def timed_drive(self, time, linear, angular, **kwargs): ) return self.__drive(req, **kwargs) - def _look(self, pitch=0, yaw=0): - if pitch < -math.pi/3: - pitch = -math.pi/3 - elif pitch > math.pi/3: - pitch = math.pi/3 + @sync(joint_lock) + def _look(self, pitch=0.0, yaw=0.0): + if pitch < -math.pi / 3: + pitch = -math.pi / 3 + elif pitch > math.pi / 3: + pitch = math.pi / 3 if yaw < -math.pi: - yaw = -math.pi + yaw = -math.pi elif yaw > math.pi: - yaw = math.pi + yaw = math.pi self.sensor_pitch_control_topic.publish(pitch) # Up and Down self.sensor_yaw_control_topic.publish(yaw) # Left and Right @@ -510,19 +521,19 @@ def lookUp(self): self._look(-math.pi / 8.0) def lookForward(self): - self._look(0,0) + self._look(0, 0) def lookDown(self): self._look(math.pi / 4.0) - + def lookRight(self): - self._look(0,-math.pi/2) - + self._look(0, -math.pi / 2) + def lookLeft(self): - self._look(0,math.pi/2) - + self._look(0, math.pi / 2) + def lookBack(self): - self._look(0,math.pi) + self._look(0, math.pi) # # # EXCAVATOR SPECIFIC CODE # # # def bucket_info(self): @@ -530,6 +541,7 @@ def bucket_info(self): rospy.logerr("bucket_info:" + self.rover_type + " is not an excavator") return self.bucket_info_msg # last message from the bucket_info topic bucket_info srcp2_msgs/ExcavatorMsg + @sync(joint_lock) def move_shoulder_yaw(self, angle): """ shoulder_yaw "#1" has full horizontal rotation motion Allows the rover "excavator" move volatiles between volatile and hauler without needing to move the wheels @@ -546,6 +558,7 @@ def move_shoulder_yaw(self, angle): angle = -math.pi # min self.shoulder_yaw_control.publish(angle) # publishes angle on the shoulder_yaw_joint_controller/command topic + @sync(joint_lock) def move_shoulder_pitch(self, angle): """ Base Arm "#2" limited vertical motion -math.pi/5 to math.pi/3 radians Best bang for our buck, in regards to arm movement as its the biggest part @@ -556,14 +569,15 @@ def move_shoulder_pitch(self, angle): rospy.logerr("move_shoulder_pitch:" + self.rover_type + " is not an excavator") return # checking bounds - if angle > (3*math.pi / 8.0): + if angle > (3 * math.pi / 8.0): rospy.logerr("move_shoulder_pitch:" + str(angle) + " exceeds allowed limits moving to max position") - angle = 3*math.pi / 8.0 # max - elif angle < (-3*math.pi / 8.0): + angle = 3 * math.pi / 8.0 # max + elif angle < (-3 * math.pi / 8.0): rospy.logerr("move_shoulder_pitch:" + str(angle) + " exceeds allowed limits moving to minimum position") - angle = -3*math.pi / 8.0 # min + angle = -3 * math.pi / 8.0 # min self.shoulder_pitch_control.publish(angle) + @sync(joint_lock) def move_elbow_pitch(self, angle): """Distal Arm "#3" limited vertical motion -math.pi/3 to math.pi/3 radians Good for lowering the bucket @@ -573,14 +587,15 @@ def move_elbow_pitch(self, angle): rospy.logerr("move_elbow_pitch:" + self.rover_type + " is not an excavator") return # checking bounds - if angle > (7*math.pi / 8.0): #@NOTE: or its elbow_pitch_joint: 7pi/8 != pi/4 as the wiki says + if angle > (7 * math.pi / 8.0): # @NOTE: or its elbow_pitch_joint: 7pi/8 != pi/4 as the wiki says rospy.logerr("move_shoulder_pitch:" + str(angle) + " exceeds allowed limits moving to max position") - angle = 7*math.pi / 8.0 # max + angle = 7 * math.pi / 8.0 # max elif angle < (-math.pi / 4.0): rospy.logerr("move_shoulder_pitch:" + str(angle) + " exceeds allowed limits moving to minimum position") - angle = -math.pi / 4.0 # min + angle = -math.pi / 4.0 # min self.elbow_pitch_control.publish(angle) + @sync(joint_lock) def move_bucket(self, angle): # checking bounds if self.rover_type == "excavator": @@ -589,7 +604,7 @@ def move_bucket(self, angle): angle = 2 * math.pi / 3.0 # max elif angle < (-2 * math.pi / 3.0): rospy.logerr("move_bucket:" + str(angle) + " exceeds allowed limits moving to minimum position") - angle = -2 * math.pi / 3.0 # min + angle = -2 * math.pi / 3.0 # min self.bucket_control.publish(angle) return rospy.logerr("move_bucket:" + self.rover_type + " is not an excavator") @@ -627,6 +642,7 @@ def bin_info(self): return return self.score.hauler_volatile_score + @sync(joint_lock) def move_bin(self, angle): if self.rover_type != "hauler": rospy.logerr("move_bin:" + self.rover_type + " is not a hauler") From da8eae49e83450f26ac0302d52c4265ca4424aab Mon Sep 17 00:00:00 2001 From: Carter Frost Date: Mon, 5 Apr 2021 01:11:36 -0700 Subject: [PATCH 03/18] src/scoot/src/Driver.py: reenable dynamic reconfigure, added sync locks --- src/scoot/src/Driver.py | 70 ++++++++++++++++++++++++----------------- 1 file changed, 42 insertions(+), 28 deletions(-) diff --git a/src/scoot/src/Driver.py b/src/scoot/src/Driver.py index 6a28bb9..d22cd60 100755 --- a/src/scoot/src/Driver.py +++ b/src/scoot/src/Driver.py @@ -27,12 +27,22 @@ from object_detection.msg import Detection from angles import shortest_angular_distance -package_lock = threading.Lock() +driving_lock = threading.Lock() +control_lock = threading.Lock() # from Scoot import sync from Scoot import Location +class sync(object): + def __init__(self, lock): + self.lock = lock + def __call__(self, func): + def wrapped_f(*args,**kwargs): + with self.lock: + return func(*args,**kwargs) + return wrapped_f + class Task: """A robot relative place to navigate to. Expressed as r and theta""" @@ -120,6 +130,7 @@ def __init__(self): _thread.start_new_thread(self.do_initial_config, ()) def _stop_now(self, result): + self.print_debug('IDLE') self.drive(0, 0, State.DRIVE_MODE_STOP) self.CurrentState = State.STATE_IDLE while not self.Work.empty(): @@ -131,8 +142,9 @@ def _stop_now(self, result): if self.Doing is not None: self.Doing.result = result - + @sync(control_lock) def _control(self, req): + rospy.loginfo("_control: Called") self.current_distance = float('inf') self.current_obstacles = 0 self.current_obstacle_data = 0 @@ -142,16 +154,17 @@ def _control(self, req): r = req.req[-1] t = Task(r, True) self.Work.put(t, True) - + sleep_wait = 0.2 sleep_turns = r.timeout / sleep_wait - #while not t.sema.acquire(blocking=False): - # rospy.sleep(sleep_wait) - # sleep_turns -= 1 - # if sleep_turns == 0: - # # Ugh. Is this safe? - # with package_lock: - # self._stop_now(MoveResult.TIMEOUT) + while not self.Work.empty() or self.Doing is not None or self.Goal is not None: + with driving_lock: # try to stay waiting until a drive is finished + pass + rospy.sleep(sleep_wait) + #sleep_turns -= 1 + #if sleep_turns == 0: + # with driving_lock: + # self._stop_now(MoveResult.TIMEOUT) rval = MoveResult() rval.result = t.result @@ -163,25 +176,26 @@ def _control(self, req): self.current_obstacle_data = 0 self.current_distance = float('inf') self.obstacle_heading = 0 + rospy.loginfo("_control: right before return") return rval - # @sync(package_lock) - # def _reconfigure(self, config, level): - # State.DRIVE_SPEED = config["DRIVE_SPEED"] - # State.REVERSE_SPEED = config["REVERSE_SPEED"] - # State.TURN_SPEED = config["TURN_SPEED"] - # State.HEADING_RESTORE_FACTOR = config["HEADING_RESTORE_FACTOR"] - # State.GOAL_DISTANCE_OK = config["GOAL_DISTANCE_OK"] - # State.ROTATE_THRESHOLD = config["ROTATE_THRESHOLD"] - # State.DRIVE_ANGLE_ABORT = config["DRIVE_ANGLE_ABORT"] - # self.print_debug('Mobility parameter reconfiguration done.') - # return config - - # @sync(package_lock) + @sync(driving_lock) + def _reconfigure(self, config, level): + State.DRIVE_SPEED = config["DRIVE_SPEED"] + State.REVERSE_SPEED = config["REVERSE_SPEED"] + State.TURN_SPEED = config["TURN_SPEED"] + State.HEADING_RESTORE_FACTOR = config["HEADING_RESTORE_FACTOR"] + State.GOAL_DISTANCE_OK = config["GOAL_DISTANCE_OK"] + State.ROTATE_THRESHOLD = config["ROTATE_THRESHOLD"] + State.DRIVE_ANGLE_ABORT = config["DRIVE_ANGLE_ABORT"] + self.print_debug('Mobility parameter reconfiguration done.') + return config + + # @sync(driving_lock) # def _joystick(self, joy_command): # self.JoystickCommand = joy_command - # @sync(package_lock) + @sync(driving_lock) def set_mode(self, msg): if msg.data == 1: self._stop_now(MoveResult.USER_ABORT) @@ -209,7 +223,7 @@ def __check_obstacles(self): self._stop_now(MoveResult.HOME_FIDUCIAL) self.print_debug("__check_obstacles: MoveResult.HOME_FIDUCIAL") - # @sync(package_lock) + @sync(driving_lock) def _obstacle(self, msg): self.current_obstacles &= ~msg.mask self.current_obstacles |= msg.msg @@ -220,7 +234,7 @@ def _obstacle(self, msg): self.current_obstacle_data = msg.data self.__check_obstacles() - # @sync(package_lock) + @sync(driving_lock) def _vision(self, msg): rospy.loginfo("Driver.py's _vision called with:") rospy.loginfo(msg) @@ -231,7 +245,7 @@ def _vision(self, msg): rospy.set_param("/"+self.rover_name+"/cubesat_point_from_rover", {'x': msg.x, 'y': msg.y, 'z': msg.z}) self.__check_obstacles() - # @sync(package_lock) + @sync(driving_lock) def _odom(self, msg): self.OdomLocation.Odometry = msg @@ -251,7 +265,7 @@ def print_debug(self, msg): # self.state_machine.publish(s) # self.dbg_msg = msg - # @sync(package_lock) + @sync(driving_lock) def run(self): if self.CurrentState == State.STATE_IDLE: # self.print_debug('IDLE') From f2e4d6fa1a85ab51c98871a885c6240cdb24e5c6 Mon Sep 17 00:00:00 2001 From: Carter Frost Date: Mon, 5 Apr 2021 10:35:16 -0700 Subject: [PATCH 04/18] Scoot package: massive name refactors, spelling, import cleanup, PEP8 --- src/scoot/src/Driver.py | 151 ++++++++---------- src/scoot/src/Scoot.py | 117 +++++++------- src/scoot/src/WaypointNavigator.py | 49 +++--- .../src/behaviors/excavator/goto_volatile.py | 2 +- .../behaviors/excavator/waypoint_search.py | 4 +- src/scoot/src/behaviors/go_to.py | 9 +- src/scoot/src/behaviors/scout/DDSA.py | 67 ++++---- .../behaviors/scout/goto_processing_plant.py | 6 +- .../src/behaviors/scout/goto_waypoint.py | 4 +- .../src/behaviors/scout/home_alignment.py | 50 +++--- .../src/behaviors/scout/searchRandomWalk.py | 4 +- src/scoot/src/repl.py | 9 +- src/scoot/src/test_square.py | 53 +++--- 13 files changed, 266 insertions(+), 259 deletions(-) diff --git a/src/scoot/src/Driver.py b/src/scoot/src/Driver.py index d22cd60..39032dc 100755 --- a/src/scoot/src/Driver.py +++ b/src/scoot/src/Driver.py @@ -1,47 +1,33 @@ #! /usr/bin/env python3 - -import sys - -import numpy import rospy import angles import math -import copy import _thread import threading - from multiprocessing import Queue # or import queue -import tf -# from sensor_msgs.msg import Joy -from std_msgs.msg import UInt8, String, Float32 from nav_msgs.msg import Odometry from geometry_msgs.msg import Twist, Pose2D, Point -# from dynamic_reconfigure.server import Server -# from dynamic_reconfigure.client import Client -from srcp2_msgs import msg, srv -# from mobility.cfg import driveConfig -# from mobility.srv import Core from scoot.srv import Core from scoot.msg import MoveResult from obstacle.msg import Obstacles from object_detection.msg import Detection -from angles import shortest_angular_distance +from Scoot import Location driving_lock = threading.Lock() control_lock = threading.Lock() -# from Scoot import sync -from Scoot import Location +class Sync(object): + def __init__(self, lock): + self.lock = lock + + def __call__(self, func): + def wrapped_f(*args, **kwargs): + with self.lock: + return func(*args, **kwargs) + + return wrapped_f -class sync(object): - def __init__(self, lock): - self.lock = lock - def __call__(self, func): - def wrapped_f(*args,**kwargs): - with self.lock: - return func(*args,**kwargs) - return wrapped_f class Task: """A robot relative place to navigate to. Expressed as r and theta""" @@ -58,6 +44,15 @@ def __init__(self, msg, blocking=True): """ +def print_debug(msg): + rospy.loginfo(msg) + # if self.dbg_msg is None or self.dbg_msg != msg: + # s = String() + # s.data = msg + # self.state_machine.publish(s) + # self.dbg_msg = msg + + class State: """Global robot state variables""" @@ -79,7 +74,7 @@ class State: ROTATE_THRESHOLD = 0 DRIVE_ANGLE_ABORT = 0 - DRIVE_SPEED_MAX = 2*math.pi + DRIVE_SPEED_MAX = 2 * math.pi TURN_SPEED_MAX = 1.2 def __init__(self): @@ -100,15 +95,17 @@ def __init__(self): # self.JoystickCommand.axes = [0,0,0,0,0,0] self.rover_name = rospy.get_param('rover_name', default='small_scout_1') - + # Configuration - State.DRIVE_SPEED = rospy.get_param("/"+self.rover_name+"/Core/DRIVE_SPEED", default=5) - State.REVERSE_SPEED = rospy.get_param("/"+self.rover_name+"/Core/REVERSE_SPEED", default=5) - State.TURN_SPEED = rospy.get_param("/"+self.rover_name+"/Core/TURN_SPEED", default=5) - State.HEADING_RESTORE_FACTOR = rospy.get_param("/"+self.rover_name+"/Core/HEADING_RESTORE_FACTOR", default=2) - State.GOAL_DISTANCE_OK = rospy.get_param("/"+self.rover_name+"/Core/GOAL_DISTANCE_OK", default=0.1) - State.ROTATE_THRESHOLD = rospy.get_param("/"+self.rover_name+"/Core/ROTATE_THRESHOLD", default=math.pi / 16) - State.DRIVE_ANGLE_ABORT = rospy.get_param("/"+self.rover_name+"/Core/DRIVE_ANGLE_ABORT", default=math.pi / 4) + State.DRIVE_SPEED = rospy.get_param("/" + self.rover_name + "/Core/DRIVE_SPEED", default=5) + State.REVERSE_SPEED = rospy.get_param("/" + self.rover_name + "/Core/REVERSE_SPEED", default=5) + State.TURN_SPEED = rospy.get_param("/" + self.rover_name + "/Core/TURN_SPEED", default=5) + State.HEADING_RESTORE_FACTOR = rospy.get_param("/" + self.rover_name + "/Core/HEADING_RESTORE_FACTOR", + default=2) + State.GOAL_DISTANCE_OK = rospy.get_param("/" + self.rover_name + "/Core/GOAL_DISTANCE_OK", default=0.1) + State.ROTATE_THRESHOLD = rospy.get_param("/" + self.rover_name + "/Core/ROTATE_THRESHOLD", default=math.pi / 16) + State.DRIVE_ANGLE_ABORT = rospy.get_param("/" + self.rover_name + "/Core/DRIVE_ANGLE_ABORT", + default=math.pi / 4) # Subscribers # rospy.Subscriber('joystick', Joy, self._joystick, queue_size=10) @@ -130,7 +127,7 @@ def __init__(self): _thread.start_new_thread(self.do_initial_config, ()) def _stop_now(self, result): - self.print_debug('IDLE') + print_debug('IDLE') self.drive(0, 0, State.DRIVE_MODE_STOP) self.CurrentState = State.STATE_IDLE while not self.Work.empty(): @@ -142,7 +139,7 @@ def _stop_now(self, result): if self.Doing is not None: self.Doing.result = result - @sync(control_lock) + @Sync(control_lock) def _control(self, req): rospy.loginfo("_control: Called") self.current_distance = float('inf') @@ -154,17 +151,17 @@ def _control(self, req): r = req.req[-1] t = Task(r, True) self.Work.put(t, True) - + sleep_wait = 0.2 sleep_turns = r.timeout / sleep_wait while not self.Work.empty() or self.Doing is not None or self.Goal is not None: - with driving_lock: # try to stay waiting until a drive is finished - pass - rospy.sleep(sleep_wait) - #sleep_turns -= 1 - #if sleep_turns == 0: - # with driving_lock: - # self._stop_now(MoveResult.TIMEOUT) + with driving_lock: # try to stay waiting until a drive is finished + pass + rospy.sleep(sleep_wait) + # sleep_turns -= 1 + # if sleep_turns == 0: + # with driving_lock: + # self._stop_now(MoveResult.TIMEOUT) rval = MoveResult() rval.result = t.result @@ -179,7 +176,7 @@ def _control(self, req): rospy.loginfo("_control: right before return") return rval - @sync(driving_lock) + @Sync(driving_lock) def _reconfigure(self, config, level): State.DRIVE_SPEED = config["DRIVE_SPEED"] State.REVERSE_SPEED = config["REVERSE_SPEED"] @@ -188,14 +185,15 @@ def _reconfigure(self, config, level): State.GOAL_DISTANCE_OK = config["GOAL_DISTANCE_OK"] State.ROTATE_THRESHOLD = config["ROTATE_THRESHOLD"] State.DRIVE_ANGLE_ABORT = config["DRIVE_ANGLE_ABORT"] - self.print_debug('Mobility parameter reconfiguration done.') - return config + print_debug('Mobility parameter reconfiguration done.') + return config + + # @sync(driving_lock) - # @sync(driving_lock) # def _joystick(self, joy_command): # self.JoystickCommand = joy_command - @sync(driving_lock) + @Sync(driving_lock) def set_mode(self, msg): if msg.data == 1: self._stop_now(MoveResult.USER_ABORT) @@ -206,24 +204,24 @@ def __check_obstacles(self): if (detected & Obstacles.IS_LIDAR) != 0: self._stop_now(MoveResult.OBSTACLE_LASER) - self.print_debug("__check_obstacles: MoveResult.OBSTACLE_LASER") + print_debug("__check_obstacles: MoveResult.OBSTACLE_LASER") elif (detected & Obstacles.IS_VOLATILE) != 0: self._stop_now(MoveResult.OBSTACLE_VOLATILE) - self.print_debug("__check_obstacles: MoveResult.OBSTACLE_VOLATILE") + print_debug("__check_obstacles: MoveResult.OBSTACLE_VOLATILE") elif (detected & Obstacles.VISION_VOLATILE) != 0: self._stop_now(MoveResult.VISION_VOLATILE) - self.print_debug("__check_obstacles: MoveResult.VISION_VOLATILE") + print_debug("__check_obstacles: MoveResult.VISION_VOLATILE") elif (detected & Obstacles.CUBESAT) != 0: self._stop_now(MoveResult.CUBESAT) - self.print_debug("__check_obstacles: MoveResult.CUBESAT") + print_debug("__check_obstacles: MoveResult.CUBESAT") elif (detected & Obstacles.HOME_LEG) != 0: self._stop_now(MoveResult.HOME_LEG) - self.print_debug("__check_obstacles: MoveResult.HOME_LEG") + print_debug("__check_obstacles: MoveResult.HOME_LEG") elif (detected & Obstacles.HOME_FIDUCIAL) != 0: self._stop_now(MoveResult.HOME_FIDUCIAL) - self.print_debug("__check_obstacles: MoveResult.HOME_FIDUCIAL") + print_debug("__check_obstacles: MoveResult.HOME_FIDUCIAL") - @sync(driving_lock) + @Sync(driving_lock) def _obstacle(self, msg): self.current_obstacles &= ~msg.mask self.current_obstacles |= msg.msg @@ -234,7 +232,7 @@ def _obstacle(self, msg): self.current_obstacle_data = msg.data self.__check_obstacles() - @sync(driving_lock) + @Sync(driving_lock) def _vision(self, msg): rospy.loginfo("Driver.py's _vision called with:") rospy.loginfo(msg) @@ -242,10 +240,10 @@ def _vision(self, msg): self.current_distance = msg.distance self.obstacle_heading = msg.heading if msg.detection_id == Obstacles.CUBESAT: - rospy.set_param("/"+self.rover_name+"/cubesat_point_from_rover", {'x': msg.x, 'y': msg.y, 'z': msg.z}) + rospy.set_param("/" + self.rover_name + "/cubesat_point_from_rover", {'x': msg.x, 'y': msg.y, 'z': msg.z}) self.__check_obstacles() - - @sync(driving_lock) + + @Sync(driving_lock) def _odom(self, msg): self.OdomLocation.Odometry = msg @@ -256,16 +254,7 @@ def drive(self, linear, angular, mode): t.angular.z = angular self.driveControl.publish(t) - def print_debug(self, msg): - rospy.loginfo(msg) - - # if self.dbg_msg is None or self.dbg_msg != msg: - # s = String() - # s.data = msg - # self.state_machine.publish(s) - # self.dbg_msg = msg - - @sync(driving_lock) + @Sync(driving_lock) def run(self): if self.CurrentState == State.STATE_IDLE: # self.print_debug('IDLE') @@ -317,7 +306,7 @@ def run(self): elif self.Doing.request.angular <= 0: self.Doing.request.angular = State.TURN_SPEED - cur = self.OdomLocation.getPose() + cur = self.OdomLocation.get_pose() self.Goal = Pose2D() self.Goal.theta = cur.theta + req_theta self.Goal.x = cur.x + req_r * math.cos(self.Goal.theta) @@ -331,9 +320,9 @@ def run(self): self.__check_obstacles() elif self.CurrentState == State.STATE_TURN: - self.print_debug('TURN') + print_debug('TURN') self.__check_obstacles() - cur = self.OdomLocation.getPose() + cur = self.OdomLocation.get_pose() heading_error = angles.shortest_angular_distance(cur.theta, self.Goal.theta) if abs(heading_error) > State.ROTATE_THRESHOLD: if heading_error < 0: @@ -345,13 +334,14 @@ def run(self): self.drive(0, 0, State.DRIVE_MODE_STOP) elif self.CurrentState == State.STATE_DRIVE: - self.print_debug('DRIVE') + print_debug('DRIVE') self.__check_obstacles() - cur = self.OdomLocation.getPose() + cur = self.OdomLocation.get_pose() heading_error = angles.shortest_angular_distance(cur.theta, self.Goal.theta) goal_angle = angles.shortest_angular_distance(cur.theta, math.atan2(self.Goal.y - cur.y, self.Goal.x - cur.x)) - if self.OdomLocation.atGoal(self.Goal, State.GOAL_DISTANCE_OK) or abs(goal_angle) > State.DRIVE_ANGLE_ABORT: + if self.OdomLocation.at_goal(self.Goal, State.GOAL_DISTANCE_OK) or abs( + goal_angle) > State.DRIVE_ANGLE_ABORT: self.Goal = None self.CurrentState = State.STATE_IDLE self.drive(0, 0, State.DRIVE_MODE_STOP) @@ -365,13 +355,14 @@ def run(self): State.DRIVE_MODE_PID) elif self.CurrentState == State.STATE_REVERSE: - self.print_debug('REVERSE') + print_debug('REVERSE') self.__check_obstacles() - cur = self.OdomLocation.getPose() + cur = self.OdomLocation.get_pose() heading_error = angles.shortest_angular_distance(cur.theta, self.Goal.theta) goal_angle = angles.shortest_angular_distance(math.pi + cur.theta, math.atan2(self.Goal.y - cur.y, self.Goal.x - cur.x)) - if self.OdomLocation.atGoal(self.Goal, State.GOAL_DISTANCE_OK) or abs(goal_angle) > State.DRIVE_ANGLE_ABORT: + if self.OdomLocation.at_goal(self.Goal, State.GOAL_DISTANCE_OK) or abs( + goal_angle) > State.DRIVE_ANGLE_ABORT: self.Goal = None self.CurrentState = State.STATE_IDLE self.drive(0, 0, State.DRIVE_MODE_STOP) @@ -384,7 +375,7 @@ def run(self): State.DRIVE_MODE_PID) elif self.CurrentState == State.STATE_TIMED: - self.print_debug('TIMED') + print_debug('TIMED') self.__check_obstacles() if self.Doing.request.linear == 0 and self.Doing.request.angular == 0: self.drive(0, 0, State.DRIVE_MODE_STOP) diff --git a/src/scoot/src/Scoot.py b/src/scoot/src/Scoot.py index 26dd7ec..12d8c17 100755 --- a/src/scoot/src/Scoot.py +++ b/src/scoot/src/Scoot.py @@ -4,21 +4,18 @@ import angles import tf import threading -import numpy from rospy import ServiceException from srcp2_msgs import msg, srv from sensor_msgs.msg import JointState -from std_msgs.msg import String, Float64 -from geometry_msgs.msg import Twist, Pose2D, Point, PoseWithCovariance, PoseWithCovarianceStamped, PoseStamped, \ +from std_msgs.msg import Float64 +from geometry_msgs.msg import Pose2D, Point, PoseWithCovarianceStamped, PoseStamped, \ Quaternion, Pose from nav_msgs.msg import Odometry from scoot.msg import MoveResult, MoveRequest from scoot.srv import Core -from functools import wraps - odom_lock = threading.Lock() drive_lock = threading.Lock() joint_lock = threading.Lock() @@ -81,7 +78,7 @@ class TimeoutException(DriveException): pass -class sync(object): +class Sync(object): def __init__(self, lock): self.lock = lock @@ -99,14 +96,14 @@ class Location: def __init__(self, odo): self.Odometry = odo - def getPose(self): # TODO: add a ros warning if self.Odometry none - '''Return a std_msgs.Pose from this Location. Useful because Pose + def get_pose(self): # TODO: add a ros warning if self.Odometry none + """Return a std_msgs.Pose from this Location. Useful because Pose has angles represented as roll, pitch, yaw. Returns: * (`std_msgs.msg.Pose`) The pose. - ''' + """ if self.Odometry is None: quat = [0, 0, 0, 0, ] else: @@ -128,7 +125,7 @@ def getPose(self): # TODO: add a ros warning if self.Odometry none return pose - def atGoal(self, goal, distance): + def at_goal(self, goal, distance): """Determine if the pose is within acceptable distance of this location Returns: @@ -149,8 +146,8 @@ class Scoot(object): """ def __init__(self, rover): - self.rover_name = None - self.rover_type = None + self.rover_name = rover # @note this will get overwritten but should still be correct + self.rover_type = self.rover_name.split("_")[1] # @note this will get overwritten but should still be correct self.TURN_SPEED = 0 self.DRIVE_SPEED = 0 self.REVERSE_SPEED = 0 @@ -260,21 +257,21 @@ def start(self, **kwargs): self.xform = tf.TransformListener() rospy.loginfo("Scoot Ready") - @sync(odom_lock) - def _odom(self, msg): - self.OdomLocation.Odometry = msg + @Sync(odom_lock) + def _odom(self, message): + self.OdomLocation.Odometry = message - def _bin_info(self, msg): - self.bin_info_msg = msg + def _bin_info(self, message): + self.bin_info_msg = message - def _bucket_info(self, msg): - self.bucket_info_msg = msg + def _bucket_info(self, message): + self.bucket_info_msg = message - def _joint_states(self, msg): - self.joint_states = msg + def _joint_states(self, message): + self.joint_states = message - def _score(self, msg): - self.score = msg + def _score(self, message): + self.score = message def get_joint_states(self): return self.joint_states @@ -285,13 +282,13 @@ def get_joint_pos(self, joint_name): rospy.logerr("get_joint_state: unknown joint:" + str(joint_name)) rospy.loginfo("get_joint_state: valid joints" + str(self.joint_states.name)) - @sync(odom_lock) - def getOdomLocation(self): + @Sync(odom_lock) + def get_odom_location(self): with odom_lock: return self.OdomLocation - @sync(odom_lock) - def getTruePose(self): + @Sync(odom_lock) + def get_true_pose(self): if self.truePoseCalled: print("True pose already called once.") # @TODO if the rover has moved 2m+ might be more useful to apply the offset to odom and return that @@ -342,16 +339,16 @@ def transform_pose(self, target_frame, pose, timeout=3.0): return self.xform.transformPose(target_frame, pose) - def getControlData(self): + def get_control_data(self): return self.control_data - def getVolPose(self): + def get_volatile_pose(self): pose_stamped = PoseWithCovarianceStamped() pose_stamped.header.frame_id = '/small_scout_1_tf/chassis' pose_stamped.header.stamp = rospy.Time.now() odom_p = self.OdomLocation.Odometry.pose.pose.position odom_o = self.OdomLocation.Odometry.pose.pose.orientation - self.getTruePose() + self.get_true_pose() offset_pos = self.world_offset.position # Point offset_ori = self.world_offset.orientation # Quaternion pose_stamped.pose.pose.position = Point(odom_p.x + offset_pos.x, odom_p.y + offset_pos.y, @@ -378,24 +375,24 @@ def getVolPose(self): # forward offset allows us to have a fixed additional distance to drive. Can be negative to underdrive to a # location. Motivated by the claw extension. def drive_to(self, place, forward_offset=0, **kwargs): - '''Drive directly to a particular point in space. The point must be in - the odometry reference frame. - + """Drive directly to a particular point in space. The point must be in + the odometry reference frame. + Arguments: - - * `place`: (`geometry_msgs.msg.Point` or `geometry_msgs.msg.Pose2D`): The place to drive. - # This is OK becasue they have the same member variables. - # Being agnostic about the type allows us to handle Pose2D and Point messages with the same code. - # Actually just requires that the object has TODO member variables and methods. + + * `place`: (`geometry_msgs.msg.Point` or `geometry_msgs.msg.Pose2D`): The place to drive. + # This is OK because they have the same member variables. + # Being agnostic about the type allows us to handle Pose2D and Point messages with the same code. + # Actually just requires that the object has TODO member variables and methods. Keyword Arguments/Returns/Raises: - - * See `mobility.swarmie.Swarmie.drive` + + * See `scoot.scoot.drive` * forward_offset to the odometry reference frame. Appropriate value to be passed in, otherwise the reference frame remains unchanged. - - ''' - loc = self.getOdomLocation().getPose() + + """ + loc = self.get_odom_location().get_pose() dist = math.hypot(loc.y - place.y, loc.x - place.x) angle = angles.shortest_angular_distance(loc.theta, math.atan2(place.y - loc.y, @@ -416,15 +413,15 @@ def drive_to(self, place, forward_offset=0, **kwargs): return self.__drive(req, **kwargs) def set_heading(self, heading, **kwargs): - '''Turn to face an absolute heading in radians. (zero is east) + """Turn to face an absolute heading in radians. (zero is east) Arguments: * `heading`: (`float`) The heading in radians. - ''' - loc = self.getOdomLocation().getPose() + """ + loc = self.get_odom_location().get_pose() angle = angles.shortest_angular_distance(loc.theta, heading) self.turn(angle, **kwargs) - @sync(drive_lock) + @Sync(drive_lock) def __drive(self, request, **kwargs): request.obstacles = ~0 if 'ignore' in kwargs: @@ -504,7 +501,7 @@ def timed_drive(self, time, linear, angular, **kwargs): ) return self.__drive(req, **kwargs) - @sync(joint_lock) + @Sync(joint_lock) def _look(self, pitch=0.0, yaw=0.0): if pitch < -math.pi / 3: pitch = -math.pi / 3 @@ -517,22 +514,22 @@ def _look(self, pitch=0.0, yaw=0.0): self.sensor_pitch_control_topic.publish(pitch) # Up and Down self.sensor_yaw_control_topic.publish(yaw) # Left and Right - def lookUp(self): + def look_up(self): self._look(-math.pi / 8.0) - def lookForward(self): + def look_forward(self): self._look(0, 0) - def lookDown(self): + def look_down(self): self._look(math.pi / 4.0) - def lookRight(self): + def look_right(self): self._look(0, -math.pi / 2) - def lookLeft(self): + def look_left(self): self._look(0, math.pi / 2) - def lookBack(self): + def look_back(self): self._look(0, math.pi) # # # EXCAVATOR SPECIFIC CODE # # # @@ -541,7 +538,7 @@ def bucket_info(self): rospy.logerr("bucket_info:" + self.rover_type + " is not an excavator") return self.bucket_info_msg # last message from the bucket_info topic bucket_info srcp2_msgs/ExcavatorMsg - @sync(joint_lock) + @Sync(joint_lock) def move_shoulder_yaw(self, angle): """ shoulder_yaw "#1" has full horizontal rotation motion Allows the rover "excavator" move volatiles between volatile and hauler without needing to move the wheels @@ -558,7 +555,7 @@ def move_shoulder_yaw(self, angle): angle = -math.pi # min self.shoulder_yaw_control.publish(angle) # publishes angle on the shoulder_yaw_joint_controller/command topic - @sync(joint_lock) + @Sync(joint_lock) def move_shoulder_pitch(self, angle): """ Base Arm "#2" limited vertical motion -math.pi/5 to math.pi/3 radians Best bang for our buck, in regards to arm movement as its the biggest part @@ -577,7 +574,7 @@ def move_shoulder_pitch(self, angle): angle = -3 * math.pi / 8.0 # min self.shoulder_pitch_control.publish(angle) - @sync(joint_lock) + @Sync(joint_lock) def move_elbow_pitch(self, angle): """Distal Arm "#3" limited vertical motion -math.pi/3 to math.pi/3 radians Good for lowering the bucket @@ -595,7 +592,7 @@ def move_elbow_pitch(self, angle): angle = -math.pi / 4.0 # min self.elbow_pitch_control.publish(angle) - @sync(joint_lock) + @Sync(joint_lock) def move_bucket(self, angle): # checking bounds if self.rover_type == "excavator": @@ -642,7 +639,7 @@ def bin_info(self): return return self.score.hauler_volatile_score - @sync(joint_lock) + @Sync(joint_lock) def move_bin(self, angle): if self.rover_type != "hauler": rospy.logerr("move_bin:" + self.rover_type + " is not a hauler") @@ -668,7 +665,7 @@ def get_bin_angle(self): # # # END HAULER SPECIFIC CODE # # # def get_closest_vol_pose(self): - rover_pose = self.getOdomLocation().getPose() + rover_pose = self.get_odom_location().get_pose() try: vol_list = self.vol_list_service.call() except (ServiceException, AttributeError): diff --git a/src/scoot/src/WaypointNavigator.py b/src/scoot/src/WaypointNavigator.py index 686ab71..40ed6c1 100644 --- a/src/scoot/src/WaypointNavigator.py +++ b/src/scoot/src/WaypointNavigator.py @@ -1,51 +1,58 @@ #! /usr/bin/env python3 -import angles, math, tf, rospy +import angles +import math +import rospy +import tf +from geometry_msgs.msg import Point -def toRadians(degree): + +def to_radians(degree): return 2 * math.pi * degree / 360.0 -def prettyPoint(point): + +def pretty_point(point): return "({0:0.2f}, {1:0.2f})".format(point.x, point.y) + class WaypointNavigator: def __init__(self, scoot): self.scoot = scoot def navigate(self, waypoints): - for i in range(len(waypoints)) : + for i in range(len(waypoints)): rospy.loginfo("Going to waypoint {}".format(i)) - self.navigateToWaypoint(waypoints[i]) + self.navigate_to_waypoint(waypoints[i]) - def get2DPose(self): + def get_2d_pose(self): rotation = self.scoot.OdomLocation.Odometry.pose.pose.orientation euler = tf.transformations.euler_from_quaternion(quaternion=(rotation.x, rotation.y, rotation.z, rotation.w)) return euler[2] - - def navigateToWaypoint(self, waypoint): + def navigate_to_waypoint(self, waypoint): distance = float("inf") - while distance > 0.9 : - posePosition = self.scoot.OdomLocation.Odometry.pose.pose.position - poseAngle = self.get2DPose() + pose_position = Point(0, 0, 0) + while distance > 0.9: + pose_position = self.scoot.OdomLocation.Odometry.pose.pose.position + pose_angle = self.get_2d_pose() # TODO: Remove this when the IMU flip is fixed - poseAngle = -poseAngle - angleto = math.atan2(posePosition.y - waypoint.y, -posePosition.x + waypoint.x) - angle = angles.shortest_angular_distance(poseAngle, angleto) + pose_angle = -pose_angle + angle_to = math.atan2(pose_position.y - waypoint.y, -pose_position.x + waypoint.x) + angle = angles.shortest_angular_distance(pose_angle, angle_to) - if abs(angle) < toRadians(5): + if abs(angle) < to_radians(5): # Move forward rospy.logdebug("Moving forward") self.scoot.drive(1) - elif abs(angle) < toRadians(70): + elif abs(angle) < to_radians(70): # Soft Turn # TODO: Implement with forward movement rospy.logdebug("Soft Turn angle: {}".format(angle)) - self.scoot.turn(toRadians(-10 if angle > 0 else 10)) + self.scoot.turn(to_radians(-10 if angle > 0 else 10)) else: # Hard Turn rospy.logdebug("Hard Turn angle: {}".format(angle)) - self.scoot.turn(toRadians(-50 if angle > 0 else 50)) + self.scoot.turn(to_radians(-50 if angle > 0 else 50)) - posePosition = self.scoot.OdomLocation.Odometry.pose.pose.position - distance = math.hypot(waypoint.x - posePosition.x, waypoint.y - posePosition.y) - rospy.loginfo("Reached waypoint: {} at point: {}".format(prettyPoint(waypoint), prettyPoint(posePosition))) + pose_position = self.scoot.OdomLocation.Odometry.pose.pose.position + distance = math.hypot(waypoint.x - pose_position.x, waypoint.y - pose_position.y) + rospy.loginfo("Reached waypoint: {} at point: {}".format(pretty_point(waypoint), pretty_point(pose_position))) diff --git a/src/scoot/src/behaviors/excavator/goto_volatile.py b/src/scoot/src/behaviors/excavator/goto_volatile.py index 47b1c8e..0509246 100755 --- a/src/scoot/src/behaviors/excavator/goto_volatile.py +++ b/src/scoot/src/behaviors/excavator/goto_volatile.py @@ -24,7 +24,7 @@ def main(task=None): sys.exit(0) # "succeeded" # @TODO: might retest mass as this volatile might be almost exhausted # @TODO: obstacle avoidance calls should live here scoot.brake() - if scoot.OdomLocation.atGoal(vol_pose, 2.0): + if scoot.OdomLocation.at_goal(vol_pose, 2.0): sys.exit(0) # "succeeded" else: sys.exit(-1) # "failed" diff --git a/src/scoot/src/behaviors/excavator/waypoint_search.py b/src/scoot/src/behaviors/excavator/waypoint_search.py index dc26b07..84d4bf3 100755 --- a/src/scoot/src/behaviors/excavator/waypoint_search.py +++ b/src/scoot/src/behaviors/excavator/waypoint_search.py @@ -22,7 +22,7 @@ def bug_0(locations): rospy.sleep(0.5) for waypoint in locations: - while not scoot.getOdomLocation().atGoal(waypoint, distance=2): + while not scoot.get_odom_location().at_goal(waypoint, distance=2): try: rospy.loginfo('driving') scoot.drive_to(waypoint) @@ -56,7 +56,7 @@ def bug_0(locations): pass rospy.loginfo('Reached waypoint') rospy.loginfo("Waypoint: " + str(waypoint.x) + ", " + str(waypoint.y)) - rospy.loginfo("My loc: " + str(scoot.getOdomLocation().getPose().x) + ", " + str(scoot.getOdomLocation().getPose().y)) + rospy.loginfo("My loc: " + str(scoot.get_odom_location().get_pose().x) + ", " + str(scoot.get_odom_location().get_pose().y)) def main(task=None): diff --git a/src/scoot/src/behaviors/go_to.py b/src/scoot/src/behaviors/go_to.py index 988d930..78e4d67 100644 --- a/src/scoot/src/behaviors/go_to.py +++ b/src/scoot/src/behaviors/go_to.py @@ -4,18 +4,16 @@ from __future__ import print_function import sys import rospy -import math -from Scoot import Scoot -from geometry_msgs.msg import Point, Pose, Quaternion +from geometry_msgs.msg import Point from std_msgs.msg import String from Scoot import Scoot, Location -from nav_msgs.msg import Odometry scoot = None pub = None status_topic = '/small_scout_1/bug_nav_status' last_status_msg = None + def goto(x, y, timeout, tolerance): global last_status_msg global scoot @@ -31,11 +29,13 @@ def goto(x, y, timeout, tolerance): else: return False + def status_handler(msg): global last_status_msg rospy.logwarn("Received status from bug nav: " + str(msg.data)) last_status_msg = msg.data + def main(task=None): global pub global scoot @@ -54,6 +54,7 @@ def main(task=None): rospy.loginfo('shutdown') sys.exit(0) # "succeeded + if __name__ == '__main__': rospy.init_node('waypoint_stub') sys.exit(main()) diff --git a/src/scoot/src/behaviors/scout/DDSA.py b/src/scoot/src/behaviors/scout/DDSA.py index 55e47d1..2b7a1e5 100755 --- a/src/scoot/src/behaviors/scout/DDSA.py +++ b/src/scoot/src/behaviors/scout/DDSA.py @@ -1,20 +1,25 @@ #!/usr/bin/env python3 -import rospy, sys, tf -from Scoot import * -from geometry_msgs.msg import Point from enum import Enum +import math +import rospy +import sys +from Scoot import * from WaypointNavigator import WaypointNavigator +from geometry_msgs.msg import Point + class Span(Enum): WALK = 1 RANGE = 2 -def createWaypoint(x, y, altitude): + +def create_waypoint(x, y, altitude): return Point(x, y, altitude) -def calculateRange(type, start, end, length): + +def calculate_range(range_type, start, end, length): # Build the range with either a walk (steps between point 1 and 2) or range (go directly to point 2) - if type == Span.WALK: + if range_type == Span.WALK: waypoints = [] print("Calculating walk") deltax = end.x - start.x @@ -26,14 +31,15 @@ def calculateRange(type, start, end, length): start.y + (i * length * deltay / distance), start.z + (i * length * deltaz / distance))) return waypoints - elif type == Span.RANGE: + elif range_type == Span.RANGE: return [end] -def buildWaypoint(centerx, centery, xoffset, yoffset, altitude): - return createWaypoint(centerx + xoffset, centery + yoffset, altitude) -def buildDDSAWaypoints(rangeType, loops, altitude=0, size=1, index=0, radius=1, steplength=1): +def build_waypoint(centerx, centery, xoffset, yoffset, altitude): + return create_waypoint(centerx + xoffset, centery + yoffset, altitude) + +def build_DDSA_waypoints(range_type, loops, altitude=0, size=1, index=0, radius=1, step_length=1): waypoints = [] start = Point(0, 0, altitude) waypoints.append(start) @@ -41,71 +47,68 @@ def buildDDSAWaypoints(rangeType, loops, altitude=0, size=1, index=0, radius=1, for loop in range(0, loops): for corner in range(0, 4): - if (loop == 0 and corner == 0): + if loop == 0 and corner == 0: next = Point(0, index + 1, altitude) else: xoffset = 1 + index + (loop * size) yoffset = xoffset - if (corner == 0): + if corner == 0: xoffset = -(1 + index + ((loop - 1) * size)) - elif (corner == 3): + elif corner == 3: xoffset = -xoffset - if (corner == 2 or corner == 3): + if corner == 2 or corner == 3: yoffset = -yoffset next = Point(xoffset, yoffset, altitude) - for waypoint in calculateRange(rangeType, previous, next, steplength): + for waypoint in calculate_range(range_type, previous, next, step_length): waypoints.append(Point(waypoint.x * radius, waypoint.y * radius, waypoint.z)) previous = next return waypoints -# We define main beacuse task expects main() -def main( task=None ): +# We define main because task expects main() +def main(task=None): # Determine if main was called from the shell or by the task object if task: scoot = task.scoot - else: # Called from shell + else: # Called from shell # Give the node a name name = 'small_scout_1' - + scoot = Scoot(name) scoot.start(node_name='test') navigator = WaypointNavigator(scoot) - while scoot.OdomLocation.Odometry == None: - print("Waiting for inital odom...") + while scoot.OdomLocation.Odometry is None: + print("Waiting for initial odom...") rospy.sleep(1) - ddsaWaypoints = buildDDSAWaypoints(Span.RANGE, loops=5) + ddsa_waypoints = build_DDSA_waypoints(Span.RANGE, loops=5) - ddsaWaypoints = [Point(waypoint.x + scoot.OdomLocation.Odometry.pose.pose.position.x, - waypoint.y + scoot.OdomLocation.Odometry.pose.pose.position.y, - waypoint.z) for waypoint in ddsaWaypoints] + ddsa_waypoints = [Point(waypoint.x + scoot.OdomLocation.Odometry.pose.pose.position.x, + waypoint.y + scoot.OdomLocation.Odometry.pose.pose.position.y, + waypoint.z) for waypoint in ddsa_waypoints] - rospy.loginfo("Waypoints generated, size: {}".format(len(ddsaWaypoints))) + rospy.loginfo("Waypoints generated, size: {}".format(len(ddsa_waypoints))) rospy.loginfo("Navigating Waypoints...") - navigator.navigate(ddsaWaypoints) + navigator.navigate(ddsa_waypoints) # Our exit code convention is that 0 means a volatile was found # None 0 means nothing was found sys.exit(0) + # Called from the shell if __name__ == '__main__': - # Create node for DDSA rospy.init_node('ddsa_navigator') rospy.loginfo("DDSA Navigator started.") - + # Call main and make its exit code the return value of main main() sys.exit(0) - - - diff --git a/src/scoot/src/behaviors/scout/goto_processing_plant.py b/src/scoot/src/behaviors/scout/goto_processing_plant.py index f490ec8..c446d60 100644 --- a/src/scoot/src/behaviors/scout/goto_processing_plant.py +++ b/src/scoot/src/behaviors/scout/goto_processing_plant.py @@ -1,12 +1,10 @@ #! /usr/bin/env python3 """Goto Processing Plant node.""" -from __future__ import print_function import sys import rospy from Scoot import Scoot from .. import go_to - from obstacle.msg import Obstacles @@ -25,10 +23,10 @@ def main(task=None): scoot.brake() if result: - print('succeeded') + rospy.loginfo('goto_processing_plant: succeeded') sys.exit(0) else: - print('failed') + rospy.loginfo('goto_processing_plant: failed') sys.exit(1) diff --git a/src/scoot/src/behaviors/scout/goto_waypoint.py b/src/scoot/src/behaviors/scout/goto_waypoint.py index 57a8a32..43ca5f6 100644 --- a/src/scoot/src/behaviors/scout/goto_waypoint.py +++ b/src/scoot/src/behaviors/scout/goto_waypoint.py @@ -6,6 +6,7 @@ from Scoot import Scoot, Location, VolatileException, ObstacleException from .. import go_to + def main(task=None): if task: if type(task) == Scoot: @@ -16,13 +17,14 @@ def main(task=None): scoot = Scoot("small_scout_1") scoot.start(node_name='goto_waypoint') rospy.loginfo('Goto Waypoint Started') - #vol_pose = scoot.get_closest_vol_pose() + # vol_pose = scoot.get_closest_vol_pose() go_to.main() result = go_to.goto(20, 20, 0, 0) if result: sys.exit(0) # "succeeded" + if __name__ == '__main__': rospy.init_node('goto_waypoint') sys.exit(main()) diff --git a/src/scoot/src/behaviors/scout/home_alignment.py b/src/scoot/src/behaviors/scout/home_alignment.py index 8966e31..c58ceab 100755 --- a/src/scoot/src/behaviors/scout/home_alignment.py +++ b/src/scoot/src/behaviors/scout/home_alignment.py @@ -9,14 +9,15 @@ from scoot.msg import MoveResult from Scoot import Scoot, VolatileException, ObstacleException, PathException, AbortException, MoveResult -# @TODO Whiteboard test the angles are not quite right + +# @TODO Whiteboard test the angles are not quite right def side_align(distance, side, sides, start_pose, home_pose): global scoot - scoot.drive(distance, ignore=Obstacles.IS_VOLATILE) - scoot.set_heading(start_pose.theta + math.pi/2, ignore=-1) + scoot.set_heading(start_pose.theta + math.pi / 2, ignore=-1) + -# @TODO Whiteboard test the angles are not quite right +# @TODO Whiteboard test the angles are not quite right def main(task=None): global scoot if task: @@ -29,24 +30,24 @@ def main(task=None): scoot.start(node_name='home_alignment') rospy.loginfo('Home Alignment Started') - start_pose = scoot.getOdomLocation().getPose() + start_pose = scoot.get_odom_location().get_pose() - #side_len = scoot.getOdomLocation().distance(scoot.home_pose.x, scoot.home_pose.y) + # side_len = scoot.getOdomLocation().distance(scoot.home_pose.x, scoot.home_pose.y) side_len = 2 # @TODO: check if need to get closer, also what is a reasonable range - # @TODO: Set heading torwards home or use scoot.drive_to(scoot.home_pose, distance) where distance is desired - distance = side_len # @TODO test if this is a reasonable thing to do + # @TODO: Set heading towards home or use scoot.drive_to(scoot.home_pose, distance) where distance is desired + distance = side_len # @TODO test if this is a reasonable thing to do sides = 6 - scoot.set_heading(start_pose.theta + (math.pi / 2.0), ignore=Obstacles.IS_LIDAR) # @TODO: ignore= home | vision | lidar - scoot.drive(distance/2.0) + scoot.set_heading(start_pose.theta + (math.pi / 2.0), + ignore=Obstacles.IS_LIDAR) # @TODO: ignore= home | vision | lidar + scoot.drive(distance / 2.0) for i in range(1, sides): - scoot.set_heading(start_pose.theta + (i * (2.0 * math.pi / sides)), ignore=Obstacles.IS_LIDAR) # @TODO: ignore= home | vision | lidar + scoot.set_heading(start_pose.theta + (i * (2.0 * math.pi / sides)), + ignore=Obstacles.IS_LIDAR) # @TODO: ignore= home | vision | lidar scoot.drive(distance) - # @TODO: Set heading torwards home - + # @TODO: Set heading towards home - - start_pose = scoot.getOdomLocation().getPose() + start_pose = scoot.get_odom_location().get_pose() ''' sides = 6 for i in range(0,(sides+1)): @@ -55,21 +56,22 @@ def main(task=None): else: ''' - start_pose = scoot.getOdomLocation().getPose() + start_pose = scoot.get_odom_location().get_pose() - #side_len = scoot.getOdomLocation().distance(scoot.home_pose.x, scoot.home_pose.y) + # side_len = scoot.getOdomLocation().distance(scoot.home_pose.x, scoot.home_pose.y) side_len = 2 # @TODO: check if need to get closer, also what is a reasonable range - # @TODO: Set heading torwards home or use scoot.drive_to(scoot.home_pose, distance) where distance is desired - distance = side_len # @TODO test if this is a reasonable thing to do + # @TODO: Set heading towards home or use scoot.drive_to(scoot.home_pose, distance) where distance is desired + distance = side_len # @TODO test if this is a reasonable thing to do sides = 6 - scoot.set_heading(start_pose.theta + (math.pi / 2.0), ignore=Obstacles.IS_LIDAR) # @TODO: ignore= home | vision | lidar - scoot.drive(distance/2.0) + scoot.set_heading(start_pose.theta + (math.pi / 2.0), + ignore=Obstacles.IS_LIDAR) # @TODO: ignore= home | vision | lidar + scoot.drive(distance / 2.0) for i in range(1, sides): - scoot.set_heading(start_pose.theta + (i * (2.0 * math.pi / sides)), ignore=Obstacles.IS_LIDAR) # @TODO: ignore= home | vision | lidar + scoot.set_heading(start_pose.theta + (i * (2.0 * math.pi / sides)), + ignore=Obstacles.IS_LIDAR) # @TODO: ignore= home | vision | lidar scoot.drive(distance) - # @TODO: Set heading torwards home - + # @TODO: Set heading towards home sys.exit(0) # "succeeded" diff --git a/src/scoot/src/behaviors/scout/searchRandomWalk.py b/src/scoot/src/behaviors/scout/searchRandomWalk.py index d866f0a..230ec02 100755 --- a/src/scoot/src/behaviors/scout/searchRandomWalk.py +++ b/src/scoot/src/behaviors/scout/searchRandomWalk.py @@ -35,12 +35,12 @@ def wander(): rospy.loginfo("Spinning...") scoot.lookUp() # if this finds a cubesat it will go down to random_walk's except CubesatException handler - heading = scoot.getOdomLocation().getPose().theta + heading = scoot.get_odom_location().get_pose().theta scoot.timed_drive(20, 0, scoot.TURN_SPEED, ignore=ignoring) scoot.set_heading(heading, ignore=ignoring) # restore heading scoot.lookForward() rospy.loginfo("Wandering...") - # @NOTE: The cubesat has alot of false postives when not looking up + # @NOTE: The cubesat has a lot of false positives when not looking up scoot.drive(random.gauss(4, 1), ignore=ignoring | Obstacles.CUBESAT) except ObstacleException: diff --git a/src/scoot/src/repl.py b/src/scoot/src/repl.py index fed9ed0..fd338e5 100755 --- a/src/scoot/src/repl.py +++ b/src/scoot/src/repl.py @@ -1,4 +1,5 @@ #!/usr/bin/env python3 +from past.builtins import raw_input from Scoot import * from behaviors import * from obstacle.msg import Obstacles @@ -19,9 +20,9 @@ line = raw_input('>>> ') if line is not None and line != '': try: - exec (line) + exec(line) except Exception as e: - print (e) + print(e) except EOFError as e: - print ("Goodbye") - print ("Qapla'!") + print("Goodbye") + print("Qapla'!") diff --git a/src/scoot/src/test_square.py b/src/scoot/src/test_square.py index 49a6957..709081a 100755 --- a/src/scoot/src/test_square.py +++ b/src/scoot/src/test_square.py @@ -3,27 +3,29 @@ from __future__ import print_function import argparse -import sys -import math -import rospy +import sys +import math +import rospy from obstacle.msg import Obstacles from geometry_msgs.msg import Pose2D, Point from Scoot import * + def dumb_square(distance): global scoot - scoot.drive(distance, ignore=Obstacles.IS_VOLATILE) - scoot.turn(math.pi/2, ignore=Obstacles.IS_VOLATILE) + scoot.drive(distance, ignore=Obstacles.IS_VOLATILE) + scoot.turn(math.pi / 2, ignore=Obstacles.IS_VOLATILE) - scoot.drive(distance, ignore=Obstacles.IS_VOLATILE) - scoot.turn(math.pi/2, ignore=Obstacles.IS_VOLATILE) + scoot.drive(distance, ignore=Obstacles.IS_VOLATILE) + scoot.turn(math.pi / 2, ignore=Obstacles.IS_VOLATILE) - scoot.drive(distance, ignore=Obstacles.IS_VOLATILE) - scoot.turn(math.pi/2, ignore=Obstacles.IS_VOLATILE) + scoot.drive(distance, ignore=Obstacles.IS_VOLATILE) + scoot.turn(math.pi / 2, ignore=Obstacles.IS_VOLATILE) + + scoot.drive(distance, ignore=Obstacles.IS_VOLATILE) + scoot.turn(math.pi / 2, ignore=Obstacles.IS_VOLATILE) - scoot.drive(distance, ignore=Obstacles.IS_VOLATILE) - scoot.turn(math.pi/2, ignore=Obstacles.IS_VOLATILE) def smart_square(distance, ignore_lidar=False): global scoot @@ -32,20 +34,20 @@ def smart_square(distance, ignore_lidar=False): ignore |= Obstacles.IS_LIDAR # Compute a square based on the current heading. - start_pose = scoot.getOdomLocation().getPose() + start_pose = scoot.get_odom_location().get_pose() start = Point() - start.x = start_pose.x + start.x = start_pose.x start.y = start_pose.y print('Start point: ({:.2f}, {:.2f})'.format(start.x, start.y)) - + sq1 = Point() sq1.x = start.x + distance * math.cos(start_pose.theta) sq1.y = start.y + distance * math.sin(start_pose.theta) - + sq2 = Point() - sq2.x = sq1.x + distance * math.cos(start_pose.theta + math.pi/2) - sq2.y = sq1.y + distance * math.sin(start_pose.theta + math.pi/2) - + sq2.x = sq1.x + distance * math.cos(start_pose.theta + math.pi / 2) + sq2.y = sq1.y + distance * math.sin(start_pose.theta + math.pi / 2) + sq3 = Point() sq3.x = sq2.x + distance * math.cos(start_pose.theta + math.pi) sq3.y = sq2.y + distance * math.sin(start_pose.theta + math.pi) @@ -57,25 +59,27 @@ def smart_square(distance, ignore_lidar=False): scoot.set_heading(start_pose.theta) - end_pose = scoot.getOdomLocation().getPose() + end_pose = scoot.get_odom_location().get_pose() print('Start point: ({:.2f}, {:.2f})'.format(end_pose.x, end_pose.y)) + def abs_square(distance): global scoot - start_pose = scoot.getOdomLocation().getPose() - + start_pose = scoot.get_odom_location().get_pose() + scoot.drive(distance, ignore=Obstacles.IS_VOLATILE) - scoot.set_heading(start_pose.theta + math.pi/2, ignore=-1) + scoot.set_heading(start_pose.theta + math.pi / 2, ignore=-1) scoot.drive(distance, ignore=Obstacles.IS_VOLATILE) scoot.set_heading(start_pose.theta + math.pi, ignore=-1) scoot.drive(distance, ignore=Obstacles.IS_VOLATILE) - scoot.set_heading(start_pose.theta + (3 * math.pi)/2, ignore=-1) + scoot.set_heading(start_pose.theta + (3 * math.pi) / 2, ignore=-1) scoot.drive(distance, ignore=Obstacles.IS_VOLATILE) scoot.set_heading(start_pose.theta, ignore=-1) + def main(): global scoot parser = argparse.ArgumentParser( @@ -95,7 +99,8 @@ def main(): smart_square(args.distance, args.ignore_lidar) -if __name__ == '__main__' : + +if __name__ == '__main__': global scoot rospy.init_node('TestSquareNode') scoot = Scoot("small_scout_1") From 4072afbbdcd75c8cc82e87888a5a6b188d5b0491 Mon Sep 17 00:00:00 2001 From: Carter Frost Date: Tue, 13 Apr 2021 00:35:30 -0700 Subject: [PATCH 05/18] src/scoot/msg/MoveRequest.msg: added linear_y, changed linear->linear_x --- src/scoot/msg/MoveRequest.msg | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/scoot/msg/MoveRequest.msg b/src/scoot/msg/MoveRequest.msg index a543055..cb97bab 100644 --- a/src/scoot/msg/MoveRequest.msg +++ b/src/scoot/msg/MoveRequest.msg @@ -2,6 +2,7 @@ float64 r float64 theta float64 timer float64 angular -float64 linear +float64 linear_x +float64 linear_y int32 obstacles int32 timeout From c944ecbd2ac8f08f9d971fcd4bbf7be7285ab66c Mon Sep 17 00:00:00 2001 From: Carter Frost Date: Tue, 13 Apr 2021 00:37:21 -0700 Subject: [PATCH 06/18] src/scoot/src/Driver.py: initial pass as implementing strafing,+goal bug is able to strafe but there is a bug with the goal in certain directions --- src/scoot/src/Driver.py | 71 +++++++++++++++++++++-------------------- 1 file changed, 37 insertions(+), 34 deletions(-) diff --git a/src/scoot/src/Driver.py b/src/scoot/src/Driver.py index 39032dc..6b9a572 100755 --- a/src/scoot/src/Driver.py +++ b/src/scoot/src/Driver.py @@ -128,7 +128,7 @@ def __init__(self): def _stop_now(self, result): print_debug('IDLE') - self.drive(0, 0, State.DRIVE_MODE_STOP) + self.drive(0, 0, 0, State.DRIVE_MODE_STOP) self.CurrentState = State.STATE_IDLE while not self.Work.empty(): item = self.Work.get(False) @@ -247,9 +247,10 @@ def _vision(self, msg): def _odom(self, msg): self.OdomLocation.Odometry = msg - def drive(self, linear, angular, mode): + def drive(self, linear_x, linear_y, angular, mode): t = Twist() - t.linear.x = linear + t.linear.x = linear_x + t.linear.y = linear_y t.angular.y = mode t.angular.z = angular self.driveControl.publish(t) @@ -265,15 +266,6 @@ def run(self): if self.Work.empty(): pass - ''' - # Let the joystick drive. - lin = self.JoystickCommand.axes[4] * State.DRIVE_SPEED - ang = self.JoystickCommand.axes[3] * State.TURN_SPEED - if abs(lin) < 0.1 and abs(ang) < 0.1 : - self.drive(0, 0, State.DRIVE_MODE_STOP) - else: - self.drive(lin, ang, State.DRIVE_MODE_PID) - ''' else: self.Doing = self.Work.get(False) @@ -295,12 +287,12 @@ def run(self): req_r += State.GOAL_DISTANCE_OK / 2.0 elif req_r < 0: req_r -= State.GOAL_DISTANCE_OK / 2.0 - + ''' if self.Doing.request.linear > State.DRIVE_SPEED_MAX: self.Doing.request.linear = State.DRIVE_SPEED_MAX elif self.Doing.request.linear <= 0: self.Doing.request.linear = State.DRIVE_SPEED - + ''' if self.Doing.request.angular > State.TURN_SPEED_MAX: self.Doing.request.angular = State.TURN_SPEED_MAX elif self.Doing.request.angular <= 0: @@ -312,8 +304,11 @@ def run(self): self.Goal.x = cur.x + req_r * math.cos(self.Goal.theta) self.Goal.y = cur.y + req_r * math.sin(self.Goal.theta) self.Start = cur - - if self.Doing.request.r < 0: + if self.Doing.request.linear_y != 0: # STRAFING # @TODO check angles *********************** + self.Goal.x = cur.x + self.Doing.request.linear_x * math.cos(self.Goal.theta) - self.Doing.request.linear_y * math.sin(self.Goal.theta) + self.Goal.y = cur.y + self.Doing.request.linear_y * math.cos(self.Goal.theta) - self.Doing.request.linear_x * math.sin(self.Goal.theta) + self.CurrentState = State.STATE_DRIVE + elif self.Doing.request.r < 0: self.CurrentState = State.STATE_REVERSE else: self.CurrentState = State.STATE_TURN @@ -326,31 +321,39 @@ def run(self): heading_error = angles.shortest_angular_distance(cur.theta, self.Goal.theta) if abs(heading_error) > State.ROTATE_THRESHOLD: if heading_error < 0: - self.drive(0, -self.Doing.request.angular, State.DRIVE_MODE_PID) + self.drive(0, 0, -self.Doing.request.angular, State.DRIVE_MODE_PID) else: - self.drive(0, self.Doing.request.angular, State.DRIVE_MODE_PID) + self.drive(0, 0, self.Doing.request.angular, State.DRIVE_MODE_PID) else: self.CurrentState = State.STATE_DRIVE - self.drive(0, 0, State.DRIVE_MODE_STOP) + self.drive(0, 0, 0, State.DRIVE_MODE_STOP) elif self.CurrentState == State.STATE_DRIVE: - print_debug('DRIVE') + if self.Doing.request.linear_y != 0: + print_debug('STRAFE') + else: + print_debug('DRIVE') + print_debug("dist: " + str(math.hypot(self.Goal.x - self.OdomLocation.Odometry.pose.pose.position.x, + self.Goal.y - self.OdomLocation.Odometry.pose.pose.position.y))) self.__check_obstacles() cur = self.OdomLocation.get_pose() heading_error = angles.shortest_angular_distance(cur.theta, self.Goal.theta) goal_angle = angles.shortest_angular_distance(cur.theta, math.atan2(self.Goal.y - cur.y, self.Goal.x - cur.x)) - if self.OdomLocation.at_goal(self.Goal, State.GOAL_DISTANCE_OK) or abs( - goal_angle) > State.DRIVE_ANGLE_ABORT: + if self.OdomLocation.at_goal(self.Goal, State.GOAL_DISTANCE_OK) or (self.Doing.request.linear_y == 0 and + abs(goal_angle) > + State.DRIVE_ANGLE_ABORT): + if abs(goal_angle) > State.DRIVE_ANGLE_ABORT: + print_debug('goal_angle exceeded ABORT') self.Goal = None self.CurrentState = State.STATE_IDLE - self.drive(0, 0, State.DRIVE_MODE_STOP) + self.drive(0, 0, 0, State.DRIVE_MODE_STOP) elif abs(heading_error) > State.DRIVE_ANGLE_ABORT / 2: self._stop_now(MoveResult.PATH_FAIL) - self.drive(0, 0, State.DRIVE_MODE_STOP) + self.drive(0, 0, 0, State.DRIVE_MODE_STOP) else: - self.drive(self.Doing.request.linear, + self.drive(self.Doing.request.linear_x, self.Doing.request.linear_y, heading_error * State.HEADING_RESTORE_FACTOR, State.DRIVE_MODE_PID) @@ -365,26 +368,26 @@ def run(self): goal_angle) > State.DRIVE_ANGLE_ABORT: self.Goal = None self.CurrentState = State.STATE_IDLE - self.drive(0, 0, State.DRIVE_MODE_STOP) + self.drive(0, 0, 0, State.DRIVE_MODE_STOP) elif abs(heading_error) > State.DRIVE_ANGLE_ABORT / 2: self._stop_now(MoveResult.PATH_FAIL) - self.drive(0, 0, State.DRIVE_MODE_STOP) + self.drive(0, 0, 0, State.DRIVE_MODE_STOP) else: - self.drive(-State.REVERSE_SPEED, - heading_error * State.HEADING_RESTORE_FACTOR, - State.DRIVE_MODE_PID) + self.drive(-State.REVERSE_SPEED, 0, heading_error * State.HEADING_RESTORE_FACTOR, State.DRIVE_MODE_PID) elif self.CurrentState == State.STATE_TIMED: print_debug('TIMED') self.__check_obstacles() - if self.Doing.request.linear == 0 and self.Doing.request.angular == 0: - self.drive(0, 0, State.DRIVE_MODE_STOP) + if self.Doing.request.linear_x == 0 and self.Doing.request.linear_y == 0 and \ + self.Doing.request.angular == 0: + self.drive(0, 0, 0, State.DRIVE_MODE_STOP) else: - self.drive(self.Doing.request.linear, self.Doing.request.angular, State.DRIVE_MODE_PID) + self.drive(self.Doing.request.linear_x, self.Doing.request.linear_y, self.Doing.request.angular, + State.DRIVE_MODE_PID) if self.TimerCount == 0: self.CurrentState = State.STATE_IDLE - self.drive(0, 0, State.DRIVE_MODE_STOP) + self.drive(0, 0, 0, State.DRIVE_MODE_STOP) else: self.TimerCount = self.TimerCount - 1 From 2395eaea57f9ff838133f56cbefe337216ce5e0f Mon Sep 17 00:00:00 2001 From: Carter Frost Date: Tue, 13 Apr 2021 00:38:33 -0700 Subject: [PATCH 07/18] src/Scoot.py: updated for linear_x & add translate function for strafing --- src/scoot/src/Scoot.py | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/src/scoot/src/Scoot.py b/src/scoot/src/Scoot.py index 12d8c17..4b9dbeb 100755 --- a/src/scoot/src/Scoot.py +++ b/src/scoot/src/Scoot.py @@ -487,6 +487,14 @@ def drive(self, distance, **kwargs): ) return self.__drive(req, **kwargs) + def translate(self, x, y, **kwargs): + req = MoveRequest( + r=math.sqrt(x*x + y*y), + linear_x=x, + linear_y=y + ) + return self.__drive(req, **kwargs) + def turn(self, theta, **kwargs): req = MoveRequest( theta=theta, @@ -496,7 +504,7 @@ def turn(self, theta, **kwargs): def timed_drive(self, time, linear, angular, **kwargs): req = MoveRequest( timer=time, - linear=linear, + linear_x=linear, angular=angular, ) return self.__drive(req, **kwargs) From e243223e85732a062a2ded2bc7d57caa8123bce1 Mon Sep 17 00:00:00 2001 From: Carter Frost Date: Mon, 19 Apr 2021 04:12:32 -0700 Subject: [PATCH 08/18] src/scoot/launch/scoot.xml: tighten goal and thresholds --- src/scoot/launch/scoot.xml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/scoot/launch/scoot.xml b/src/scoot/launch/scoot.xml index e39a927..08f64e5 100644 --- a/src/scoot/launch/scoot.xml +++ b/src/scoot/launch/scoot.xml @@ -17,8 +17,8 @@ - - + + From abafa45622425c42f9810c52b74ae6a384df66bc Mon Sep 17 00:00:00 2001 From: Carter Frost Date: Mon, 19 Apr 2021 04:13:45 -0700 Subject: [PATCH 09/18] src/scoot/src/Core.py: remove run call will be called from _control --- src/scoot/src/Core.py | 1 - 1 file changed, 1 deletion(-) diff --git a/src/scoot/src/Core.py b/src/scoot/src/Core.py index 520d3d6..a5dd562 100755 --- a/src/scoot/src/Core.py +++ b/src/scoot/src/Core.py @@ -22,7 +22,6 @@ rospy.loginfo("Core Starting without Task") while not rospy.is_shutdown(): - driver.run() r.sleep() except rospy.ROSInterruptException: pass From 6644d6be2ea84ffc9897b765b987583b42d7e389 Mon Sep 17 00:00:00 2001 From: Carter Frost Date: Mon, 19 Apr 2021 04:14:24 -0700 Subject: [PATCH 10/18] src/scoot/msg/MoveRequest.msg: update r->x and added y parameter --- src/scoot/msg/MoveRequest.msg | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/scoot/msg/MoveRequest.msg b/src/scoot/msg/MoveRequest.msg index cb97bab..e60a19f 100644 --- a/src/scoot/msg/MoveRequest.msg +++ b/src/scoot/msg/MoveRequest.msg @@ -1,8 +1,8 @@ -float64 r +float64 x +float64 y float64 theta float64 timer float64 angular -float64 linear_x -float64 linear_y +float64 linear int32 obstacles int32 timeout From 3bbd58e9f566e1fb6a8e3d45b8d4b2934eebc61a Mon Sep 17 00:00:00 2001 From: Carter Frost Date: Mon, 19 Apr 2021 04:15:03 -0700 Subject: [PATCH 11/18] src/scoot/src/Scoot.py: updated with new MoveRequest parameters --- src/scoot/src/Scoot.py | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/src/scoot/src/Scoot.py b/src/scoot/src/Scoot.py index 4b9dbeb..41ee509 100755 --- a/src/scoot/src/Scoot.py +++ b/src/scoot/src/Scoot.py @@ -483,15 +483,14 @@ def __drive(self, request, **kwargs): def drive(self, distance, **kwargs): req = MoveRequest( - r=distance, + x=distance, ) return self.__drive(req, **kwargs) def translate(self, x, y, **kwargs): req = MoveRequest( - r=math.sqrt(x*x + y*y), - linear_x=x, - linear_y=y + x=x, + y=y ) return self.__drive(req, **kwargs) @@ -504,7 +503,7 @@ def turn(self, theta, **kwargs): def timed_drive(self, time, linear, angular, **kwargs): req = MoveRequest( timer=time, - linear_x=linear, + linear=linear, angular=angular, ) return self.__drive(req, **kwargs) From 41e5e99c15af02a952707bb1567d8b077c24ccc1 Mon Sep 17 00:00:00 2001 From: Carter Frost Date: Mon, 19 Apr 2021 04:18:33 -0700 Subject: [PATCH 12/18] src/scoot/src/Driver.py: major rewrite of driver code support blocking @TODO strafing goals, drive overshooting handlers --- src/scoot/src/Driver.py | 319 ++++++++++++++++++---------------------- 1 file changed, 147 insertions(+), 172 deletions(-) diff --git a/src/scoot/src/Driver.py b/src/scoot/src/Driver.py index 6b9a572..f88fe11 100755 --- a/src/scoot/src/Driver.py +++ b/src/scoot/src/Driver.py @@ -35,13 +35,6 @@ class Task: def __init__(self, msg, blocking=True): self.request = msg self.result = MoveResult.SUCCESS - self.sema = None - """ - if blocking: - self.sema = threading.Semaphore(0) - else: - self.sema = None - """ def print_debug(msg): @@ -91,8 +84,6 @@ def __init__(self): self.current_obstacle_data = 0 self.current_distance = float('inf') self.obstacle_heading = 0 - # self.JoystickCommand = Joy() - # self.JoystickCommand.axes = [0,0,0,0,0,0] self.rover_name = rospy.get_param('rover_name', default='small_scout_1') @@ -100,10 +91,11 @@ def __init__(self): State.DRIVE_SPEED = rospy.get_param("/" + self.rover_name + "/Core/DRIVE_SPEED", default=5) State.REVERSE_SPEED = rospy.get_param("/" + self.rover_name + "/Core/REVERSE_SPEED", default=5) State.TURN_SPEED = rospy.get_param("/" + self.rover_name + "/Core/TURN_SPEED", default=5) + State.TURN_SPEED_SLOW = rospy.get_param("/" + self.rover_name + "/Core/TURN_SPEED_SLOW", default=5) State.HEADING_RESTORE_FACTOR = rospy.get_param("/" + self.rover_name + "/Core/HEADING_RESTORE_FACTOR", default=2) State.GOAL_DISTANCE_OK = rospy.get_param("/" + self.rover_name + "/Core/GOAL_DISTANCE_OK", default=0.1) - State.ROTATE_THRESHOLD = rospy.get_param("/" + self.rover_name + "/Core/ROTATE_THRESHOLD", default=math.pi / 16) + State.ROTATE_THRESHOLD = rospy.get_param("/" + self.rover_name + "/Core/ROTATE_THRESHOLD", default=math.pi / 32) State.DRIVE_ANGLE_ABORT = rospy.get_param("/" + self.rover_name + "/Core/DRIVE_ANGLE_ABORT", default=math.pi / 4) @@ -124,7 +116,7 @@ def __init__(self): # self.config_srv = Server(driveConfig, self._reconfigure) # Start a thread to do initial configuration. - _thread.start_new_thread(self.do_initial_config, ()) + # _thread.start_new_thread(self.do_initial_config, ()) def _stop_now(self, result): print_debug('IDLE') @@ -133,15 +125,13 @@ def _stop_now(self, result): while not self.Work.empty(): item = self.Work.get(False) item.result = result - if item.sema is not None: - item.sema.release() if self.Doing is not None: self.Doing.result = result @Sync(control_lock) def _control(self, req): - rospy.loginfo("_control: Called") + rospy.loginfo("_control: Called:" + str(req)) self.current_distance = float('inf') self.current_obstacles = 0 self.current_obstacle_data = 0 @@ -154,7 +144,11 @@ def _control(self, req): sleep_wait = 0.2 sleep_turns = r.timeout / sleep_wait - while not self.Work.empty() or self.Doing is not None or self.Goal is not None: + + rospy.sleep(sleep_wait) + self.run() + """ + while not self.Work.empty() or (self.Doing is not None) or self.Goal is not None: # isinstance(arg, list) with driving_lock: # try to stay waiting until a drive is finished pass rospy.sleep(sleep_wait) @@ -162,7 +156,7 @@ def _control(self, req): # if sleep_turns == 0: # with driving_lock: # self._stop_now(MoveResult.TIMEOUT) - + """ rval = MoveResult() rval.result = t.result rval.obstacle = self.current_obstacles @@ -174,9 +168,9 @@ def _control(self, req): self.current_distance = float('inf') self.obstacle_heading = 0 rospy.loginfo("_control: right before return") + rospy.loginfo("_control rval:" + str(rval)) return rval - @Sync(driving_lock) def _reconfigure(self, config, level): State.DRIVE_SPEED = config["DRIVE_SPEED"] State.REVERSE_SPEED = config["REVERSE_SPEED"] @@ -188,12 +182,6 @@ def _reconfigure(self, config, level): print_debug('Mobility parameter reconfiguration done.') return config - # @sync(driving_lock) - - # def _joystick(self, joy_command): - # self.JoystickCommand = joy_command - - @Sync(driving_lock) def set_mode(self, msg): if msg.data == 1: self._stop_now(MoveResult.USER_ABORT) @@ -221,7 +209,6 @@ def __check_obstacles(self): self._stop_now(MoveResult.HOME_FIDUCIAL) print_debug("__check_obstacles: MoveResult.HOME_FIDUCIAL") - @Sync(driving_lock) def _obstacle(self, msg): self.current_obstacles &= ~msg.mask self.current_obstacles |= msg.msg @@ -232,7 +219,6 @@ def _obstacle(self, msg): self.current_obstacle_data = msg.data self.__check_obstacles() - @Sync(driving_lock) def _vision(self, msg): rospy.loginfo("Driver.py's _vision called with:") rospy.loginfo(msg) @@ -243,11 +229,15 @@ def _vision(self, msg): rospy.set_param("/" + self.rover_name + "/cubesat_point_from_rover", {'x': msg.x, 'y': msg.y, 'z': msg.z}) self.__check_obstacles() - @Sync(driving_lock) def _odom(self, msg): self.OdomLocation.Odometry = msg + @Sync(driving_lock) def drive(self, linear_x, linear_y, angular, mode): + print_debug('drive called: x: ' + str(linear_x)) + print_debug('drive called: y: ' + str(linear_y)) + if mode is State.DRIVE_MODE_STOP: + self.Doing = None t = Twist() t.linear.x = linear_x t.linear.y = linear_y @@ -255,153 +245,138 @@ def drive(self, linear_x, linear_y, angular, mode): t.angular.z = angular self.driveControl.publish(t) - @Sync(driving_lock) def run(self): - if self.CurrentState == State.STATE_IDLE: - # self.print_debug('IDLE') - if self.Doing is not None: - if self.Doing.sema is not None: - self.Doing.sema.release() - self.Doing = None - - if self.Work.empty(): - pass - else: - self.Doing = self.Work.get(False) - - if self.Doing.request.timer > 0: - self.TimerCount = self.Doing.request.timer * 10 - self.CurrentState = State.STATE_TIMED - else: - if self.Doing.request.r < 0: - self.Doing.request.theta = 0 - - req_theta = self.Doing.request.theta - if req_theta > 0: - req_theta += State.ROTATE_THRESHOLD / 2.0 - elif req_theta < 0: - req_theta -= State.ROTATE_THRESHOLD / 2.0 - - req_r = self.Doing.request.r - if req_r > 0: - req_r += State.GOAL_DISTANCE_OK / 2.0 - elif req_r < 0: - req_r -= State.GOAL_DISTANCE_OK / 2.0 - ''' - if self.Doing.request.linear > State.DRIVE_SPEED_MAX: - self.Doing.request.linear = State.DRIVE_SPEED_MAX - elif self.Doing.request.linear <= 0: - self.Doing.request.linear = State.DRIVE_SPEED - ''' - if self.Doing.request.angular > State.TURN_SPEED_MAX: - self.Doing.request.angular = State.TURN_SPEED_MAX - elif self.Doing.request.angular <= 0: - self.Doing.request.angular = State.TURN_SPEED + r = rospy.Rate(10) # 10hz + while self.Doing or not self.Work.empty(): + r.sleep() + if self.CurrentState == State.STATE_IDLE: + print_debug('IDLE') + if self.Work.empty(): + self.Doing = None + if not self.Work.empty(): + self.Doing = self.Work.get(False) + if self.Doing: + if self.Doing.request.timer > 0: + self.TimerCount = self.Doing.request.timer * 10 + self.CurrentState = State.STATE_TIMED + else: + self.CurrentState = State.STATE_DRIVE + req_theta = self.Doing.request.theta + cur = self.OdomLocation.get_pose() + self.Start = cur + self.Goal = Pose2D() + if req_theta == 0: + self.Doing.request.linear = State.DRIVE_SPEED + if self.Doing.request.y == 0: # Forward Backwards drive + self.Goal.theta = cur.theta + req_theta + self.Goal.x = cur.x + self.Doing.request.x * math.cos(self.Goal.theta) + self.Goal.y = cur.y + self.Doing.request.x * math.sin(self.Goal.theta) + else: # Strafing + self.Goal.theta = cur.theta + req_theta # @TODO check the math here + self.Goal.x = cur.x + self.Doing.request.x * math.cos( + cur.theta) + self.Doing.request.y * math.sin(cur.theta) + self.Goal.y = cur.y + self.Doing.request.x * math.sin( + cur.theta) + self.Doing.request.y * math.cos(cur.theta) + elif req_theta > State.ROTATE_THRESHOLD: # Turn + self.CurrentState = State.STATE_TURN + self.Goal.theta = cur.theta + req_theta + if self.Doing.request.angular > State.TURN_SPEED_MAX: + self.Doing.request.angular = State.TURN_SPEED_MAX + elif self.Doing.request.angular <= 0: + self.Doing.request.angular = State.TURN_SPEED + self.__check_obstacles() + if self.Doing: + if self.CurrentState == State.STATE_TURN: + print_debug('TURN') + self.__check_obstacles() + cur = self.OdomLocation.get_pose() + heading_error = angles.shortest_angular_distance(cur.theta, self.Goal.theta) + print_debug('heading_error:' + str(heading_error)) + print_debug("State.ROTATE_THRESHOLD:" + str(State.ROTATE_THRESHOLD)) + if math.isclose(heading_error, 0, abs_tol=0.2): + self.Doing.request.angular = State.TURN_SPEED_SLOW # CLOSE so slowing down + if math.isclose(heading_error, 0, abs_tol=0.1): + self.Doing.request.angular = State.TURN_SPEED_SLOW / 4 # SOOO CLOSE so slowing down more + if abs(heading_error) > State.ROTATE_THRESHOLD and not math.isclose(heading_error, 0, abs_tol=0.001): + if heading_error < 0: + self.drive(0, 0, -self.Doing.request.angular, State.DRIVE_MODE_PID) + else: + self.drive(0, 0, self.Doing.request.angular, State.DRIVE_MODE_PID) + else: + self.CurrentState = State.STATE_IDLE + self.drive(0, 0, 0, State.DRIVE_MODE_STOP) + elif self.CurrentState == State.STATE_DRIVE: + direction_x = 1 + direction_y = 1 + if self.Doing.request.y != 0: + print_debug('STRAFE') + else: + print_debug('DRIVE') + if self.Doing.request.y < 0: + print_debug('REVERSE Y') + # direction_y = -1 + self.__check_obstacles() cur = self.OdomLocation.get_pose() - self.Goal = Pose2D() - self.Goal.theta = cur.theta + req_theta - self.Goal.x = cur.x + req_r * math.cos(self.Goal.theta) - self.Goal.y = cur.y + req_r * math.sin(self.Goal.theta) - self.Start = cur - if self.Doing.request.linear_y != 0: # STRAFING # @TODO check angles *********************** - self.Goal.x = cur.x + self.Doing.request.linear_x * math.cos(self.Goal.theta) - self.Doing.request.linear_y * math.sin(self.Goal.theta) - self.Goal.y = cur.y + self.Doing.request.linear_y * math.cos(self.Goal.theta) - self.Doing.request.linear_x * math.sin(self.Goal.theta) - self.CurrentState = State.STATE_DRIVE - elif self.Doing.request.r < 0: - self.CurrentState = State.STATE_REVERSE + heading_error = angles.shortest_angular_distance(cur.theta, self.Goal.theta) + goal_angle = angles.shortest_angular_distance(math.pi + cur.theta, + math.atan2(self.Goal.y - cur.y, self.Goal.x - cur.x)) + print_debug("dist: " + str(math.hypot(self.Goal.x - self.OdomLocation.Odometry.pose.pose.position.x, + self.Goal.y - self.OdomLocation.Odometry.pose.pose.position.y))) + if self.OdomLocation.at_goal(self.Goal, State.GOAL_DISTANCE_OK): + print_debug('at goal') + self.Goal = None + self.CurrentState = State.STATE_IDLE + self.drive(0, 0, 0, State.DRIVE_MODE_STOP) + # elif abs(goal_angle) > State.DRIVE_ANGLE_ABORT: + # @TODO NO OVER SHOT CODE + + """ + if self.OdomLocation.at_goal(self.Goal, State.GOAL_DISTANCE_OK) or (self.Doing.request.linear_y == 0 and + abs(goal_angle) > + State.DRIVE_ANGLE_ABORT): + if abs(goal_angle) > State.DRIVE_ANGLE_ABORT: + print_debug('goal_angle exceeded ABORT') + self.Goal = None + self.CurrentState = State.STATE_IDLE + self.drive(0, 0, 0, State.DRIVE_MODE_STOP) + + elif abs(heading_error) > State.DRIVE_ANGLE_ABORT / 2: + self._stop_now(MoveResult.PATH_FAIL) + self.drive(0, 0, 0, State.DRIVE_MODE_STOP) + --------------- + + elif abs(heading_error) > State.DRIVE_ANGLE_ABORT / 2: + print_debug('heading_error exceeded') + self.CurrentState = State.STATE_IDLE + self._stop_now(MoveResult.PATH_FAIL) + self.drive(0, 0, 0, State.DRIVE_MODE_STOP) + """ + else: + if abs(self.Doing.request.y) + abs(self.Doing.request.x) == 0: + linear_x = 0 + else: + linear_x = self.Doing.request.linear * direction_x * self.Doing.request.x / ( + abs(self.Doing.request.y) + abs(self.Doing.request.x)) + if abs(self.Doing.request.y) + abs(self.Doing.request.x) == 0: + linear_y = 0 + else: + linear_y = self.Doing.request.linear * direction_y * self.Doing.request.y / ( + abs(self.Doing.request.y) + abs(self.Doing.request.x)) + self.drive(linear_x, linear_y, heading_error * State.HEADING_RESTORE_FACTOR, + State.DRIVE_MODE_PID) + + elif self.CurrentState == State.STATE_TIMED: + print_debug('TIMED') + self.__check_obstacles() + if self.Doing.request.linear == 0 and self.Doing.request.angular == 0: + self.drive(0, 0, 0, State.DRIVE_MODE_STOP) + else: + self.drive(self.Doing.request.linear, 0, self.Doing.request.angular, + State.DRIVE_MODE_PID) + + if self.TimerCount == 0: + self.CurrentState = State.STATE_IDLE + self.drive(0, 0, 0, State.DRIVE_MODE_STOP) else: - self.CurrentState = State.STATE_TURN - - self.__check_obstacles() - elif self.CurrentState == State.STATE_TURN: - print_debug('TURN') - self.__check_obstacles() - cur = self.OdomLocation.get_pose() - heading_error = angles.shortest_angular_distance(cur.theta, self.Goal.theta) - if abs(heading_error) > State.ROTATE_THRESHOLD: - if heading_error < 0: - self.drive(0, 0, -self.Doing.request.angular, State.DRIVE_MODE_PID) - else: - self.drive(0, 0, self.Doing.request.angular, State.DRIVE_MODE_PID) - else: - self.CurrentState = State.STATE_DRIVE - self.drive(0, 0, 0, State.DRIVE_MODE_STOP) - - elif self.CurrentState == State.STATE_DRIVE: - if self.Doing.request.linear_y != 0: - print_debug('STRAFE') - else: - print_debug('DRIVE') - print_debug("dist: " + str(math.hypot(self.Goal.x - self.OdomLocation.Odometry.pose.pose.position.x, - self.Goal.y - self.OdomLocation.Odometry.pose.pose.position.y))) - self.__check_obstacles() - cur = self.OdomLocation.get_pose() - heading_error = angles.shortest_angular_distance(cur.theta, self.Goal.theta) - goal_angle = angles.shortest_angular_distance(cur.theta, - math.atan2(self.Goal.y - cur.y, self.Goal.x - cur.x)) - if self.OdomLocation.at_goal(self.Goal, State.GOAL_DISTANCE_OK) or (self.Doing.request.linear_y == 0 and - abs(goal_angle) > - State.DRIVE_ANGLE_ABORT): - if abs(goal_angle) > State.DRIVE_ANGLE_ABORT: - print_debug('goal_angle exceeded ABORT') - self.Goal = None - self.CurrentState = State.STATE_IDLE - self.drive(0, 0, 0, State.DRIVE_MODE_STOP) - - elif abs(heading_error) > State.DRIVE_ANGLE_ABORT / 2: - self._stop_now(MoveResult.PATH_FAIL) - self.drive(0, 0, 0, State.DRIVE_MODE_STOP) - else: - self.drive(self.Doing.request.linear_x, self.Doing.request.linear_y, - heading_error * State.HEADING_RESTORE_FACTOR, - State.DRIVE_MODE_PID) - - elif self.CurrentState == State.STATE_REVERSE: - print_debug('REVERSE') - self.__check_obstacles() - cur = self.OdomLocation.get_pose() - heading_error = angles.shortest_angular_distance(cur.theta, self.Goal.theta) - goal_angle = angles.shortest_angular_distance(math.pi + cur.theta, - math.atan2(self.Goal.y - cur.y, self.Goal.x - cur.x)) - if self.OdomLocation.at_goal(self.Goal, State.GOAL_DISTANCE_OK) or abs( - goal_angle) > State.DRIVE_ANGLE_ABORT: - self.Goal = None - self.CurrentState = State.STATE_IDLE - self.drive(0, 0, 0, State.DRIVE_MODE_STOP) - elif abs(heading_error) > State.DRIVE_ANGLE_ABORT / 2: - self._stop_now(MoveResult.PATH_FAIL) - self.drive(0, 0, 0, State.DRIVE_MODE_STOP) - else: - self.drive(-State.REVERSE_SPEED, 0, heading_error * State.HEADING_RESTORE_FACTOR, State.DRIVE_MODE_PID) - - elif self.CurrentState == State.STATE_TIMED: - print_debug('TIMED') - self.__check_obstacles() - if self.Doing.request.linear_x == 0 and self.Doing.request.linear_y == 0 and \ - self.Doing.request.angular == 0: - self.drive(0, 0, 0, State.DRIVE_MODE_STOP) - else: - self.drive(self.Doing.request.linear_x, self.Doing.request.linear_y, self.Doing.request.angular, - State.DRIVE_MODE_PID) - - if self.TimerCount == 0: - self.CurrentState = State.STATE_IDLE - self.drive(0, 0, 0, State.DRIVE_MODE_STOP) - else: - self.TimerCount = self.TimerCount - 1 - - def do_initial_config(self): - # Do initial configuration. - params = { - "DRIVE_SPEED": State.DRIVE_SPEED, - "REVERSE_SPEED": State.REVERSE_SPEED, - "TURN_SPEED": State.TURN_SPEED, - "HEADING_RESTORE_FACTOR": State.HEADING_RESTORE_FACTOR, - "GOAL_DISTANCE_OK": State.GOAL_DISTANCE_OK, - "ROTATE_THRESHOLD": State.ROTATE_THRESHOLD, - "DRIVE_ANGLE_ABORT": State.DRIVE_ANGLE_ABORT, - } - # dyn_client = Client('scoot') - # dyn_client.update_configuration(params) - print('Initial configuration sent.') + self.TimerCount = self.TimerCount - 1 From a346870ac3c9d85e3ae26b2f9c88d2d46842e664 Mon Sep 17 00:00:00 2001 From: Carter Frost Date: Mon, 26 Apr 2021 00:11:25 -0700 Subject: [PATCH 13/18] src/scoot/launch/scoot.xml: lower drive speeds to avoid loss of contact --- src/scoot/launch/scoot.xml | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/scoot/launch/scoot.xml b/src/scoot/launch/scoot.xml index 08f64e5..3fd04f7 100644 --- a/src/scoot/launch/scoot.xml +++ b/src/scoot/launch/scoot.xml @@ -22,15 +22,15 @@ - - - + + + - + - - + + From 3bc16dd2e81e636fd6ce245628cb3d5cc47194f8 Mon Sep 17 00:00:00 2001 From: Carter Frost Date: Mon, 26 Apr 2021 00:13:20 -0700 Subject: [PATCH 14/18] src/scoot/src/Driver.py: add missing speed ros parameters --- src/scoot/src/Driver.py | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/scoot/src/Driver.py b/src/scoot/src/Driver.py index f88fe11..55c1b39 100755 --- a/src/scoot/src/Driver.py +++ b/src/scoot/src/Driver.py @@ -60,8 +60,10 @@ class State: # Tune these with dynaimc reconfigure. DRIVE_SPEED = 0 + DRIVE_SPEED_SLOW = 0 REVERSE_SPEED = 0 TURN_SPEED = 0 + TURN_SPEED_SLOW = 0 HEADING_RESTORE_FACTOR = 0 GOAL_DISTANCE_OK = 0 ROTATE_THRESHOLD = 0 @@ -84,11 +86,14 @@ def __init__(self): self.current_obstacle_data = 0 self.current_distance = float('inf') self.obstacle_heading = 0 + self.task = MoveResult.SUCCESS + self.last_twist = Twist(linear=[math.inf, math.inf]) self.rover_name = rospy.get_param('rover_name', default='small_scout_1') # Configuration State.DRIVE_SPEED = rospy.get_param("/" + self.rover_name + "/Core/DRIVE_SPEED", default=5) + State.DRIVE_SPEED_SLOW = rospy.get_param("/" + self.rover_name + "/Core/DRIVE_SPEED_SLOW", default=1) State.REVERSE_SPEED = rospy.get_param("/" + self.rover_name + "/Core/REVERSE_SPEED", default=5) State.TURN_SPEED = rospy.get_param("/" + self.rover_name + "/Core/TURN_SPEED", default=5) State.TURN_SPEED_SLOW = rospy.get_param("/" + self.rover_name + "/Core/TURN_SPEED_SLOW", default=5) From 67c67f93cedbb44752e7fc673d11093668bce3af Mon Sep 17 00:00:00 2001 From: Carter Frost Date: Mon, 26 Apr 2021 00:24:20 -0700 Subject: [PATCH 15/18] src/scoot/src/Driver.py: add task instance as class member &track result --- src/scoot/src/Driver.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/scoot/src/Driver.py b/src/scoot/src/Driver.py index 55c1b39..5d1142b 100755 --- a/src/scoot/src/Driver.py +++ b/src/scoot/src/Driver.py @@ -130,7 +130,7 @@ def _stop_now(self, result): while not self.Work.empty(): item = self.Work.get(False) item.result = result - + self.task.result = result if self.Doing is not None: self.Doing.result = result @@ -144,14 +144,14 @@ def _control(self, req): self.Work.put(Task(r, False), False) r = req.req[-1] - t = Task(r, True) - self.Work.put(t, True) + self.task = Task(r, True) + self.Work.put(self.task, True) # @TODO think about if we do want to support a drive queue sleep_wait = 0.2 sleep_turns = r.timeout / sleep_wait rospy.sleep(sleep_wait) - self.run() + self.run() # this will be a blocking call """ while not self.Work.empty() or (self.Doing is not None) or self.Goal is not None: # isinstance(arg, list) with driving_lock: # try to stay waiting until a drive is finished @@ -163,7 +163,7 @@ def _control(self, req): # self._stop_now(MoveResult.TIMEOUT) """ rval = MoveResult() - rval.result = t.result + rval.result = self.task.result rval.obstacle = self.current_obstacles rval.obstacle_data = self.current_obstacle_data rval.distance = self.current_distance From 719d19b966f08ef588065a22c740eda2be5c5231 Mon Sep 17 00:00:00 2001 From: Carter Frost Date: Mon, 26 Apr 2021 00:27:52 -0700 Subject: [PATCH 16/18] src/scoot/src/Driver.py: don't publish duplicate twists to not overwhelm the drive_controller.py, I noticed at high hz it had issues --- src/scoot/src/Driver.py | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/scoot/src/Driver.py b/src/scoot/src/Driver.py index 5d1142b..01f604e 100755 --- a/src/scoot/src/Driver.py +++ b/src/scoot/src/Driver.py @@ -248,7 +248,11 @@ def drive(self, linear_x, linear_y, angular, mode): t.linear.y = linear_y t.angular.y = mode t.angular.z = angular - self.driveControl.publish(t) + if math.isclose(t.angular.z, 0, abs_tol=.1): # practically 0 + t.angular.z = 0 + if self.last_twist != t: + self.driveControl.publish(t) + self.last_twist = t def run(self): r = rospy.Rate(10) # 10hz From 2fcc33b51ff503a94b9ce9e1c1e0f849cddb9cee Mon Sep 17 00:00:00 2001 From: Carter Frost Date: Mon, 26 Apr 2021 01:51:37 -0700 Subject: [PATCH 17/18] src/scoot/src/Driver.py: -old code, +overshoot logging, +goal checking --- src/scoot/src/Driver.py | 96 ++++++++++++++++++++--------------------- 1 file changed, 47 insertions(+), 49 deletions(-) diff --git a/src/scoot/src/Driver.py b/src/scoot/src/Driver.py index 01f604e..f4f2ffb 100755 --- a/src/scoot/src/Driver.py +++ b/src/scoot/src/Driver.py @@ -2,7 +2,6 @@ import rospy import angles import math -import _thread import threading from multiprocessing import Queue # or import queue from nav_msgs.msg import Odometry @@ -105,7 +104,6 @@ def __init__(self): default=math.pi / 4) # Subscribers - # rospy.Subscriber('joystick', Joy, self._joystick, queue_size=10) rospy.Subscriber('/' + self.rover_name + '/obstacle', Obstacles, self._obstacle) rospy.Subscriber('/' + self.rover_name + '/detections', Detection, self._vision) rospy.Subscriber('/' + self.rover_name + '/odometry/filtered', Odometry, self._odom) @@ -114,15 +112,11 @@ def __init__(self): self.control = rospy.Service('control', Core, self._control) # Publishers - # self.state_machine = rospy.Publisher('state_machine', String, queue_size=1, latch=True) self.driveControl = rospy.Publisher('/' + self.rover_name + '/cmd_vel', Twist, queue_size=10) # Configuration # self.config_srv = Server(driveConfig, self._reconfigure) - # Start a thread to do initial configuration. - # _thread.start_new_thread(self.do_initial_config, ()) - def _stop_now(self, result): print_debug('IDLE') self.drive(0, 0, 0, State.DRIVE_MODE_STOP) @@ -152,16 +146,6 @@ def _control(self, req): rospy.sleep(sleep_wait) self.run() # this will be a blocking call - """ - while not self.Work.empty() or (self.Doing is not None) or self.Goal is not None: # isinstance(arg, list) - with driving_lock: # try to stay waiting until a drive is finished - pass - rospy.sleep(sleep_wait) - # sleep_turns -= 1 - # if sleep_turns == 0: - # with driving_lock: - # self._stop_now(MoveResult.TIMEOUT) - """ rval = MoveResult() rval.result = self.task.result rval.obstacle = self.current_obstacles @@ -239,8 +223,6 @@ def _odom(self, msg): @Sync(driving_lock) def drive(self, linear_x, linear_y, angular, mode): - print_debug('drive called: x: ' + str(linear_x)) - print_debug('drive called: y: ' + str(linear_y)) if mode is State.DRIVE_MODE_STOP: self.Doing = None t = Twist() @@ -251,6 +233,8 @@ def drive(self, linear_x, linear_y, angular, mode): if math.isclose(t.angular.z, 0, abs_tol=.1): # practically 0 t.angular.z = 0 if self.last_twist != t: + print_debug('drive called: x: ' + str(linear_x)) + print_debug('drive called: y: ' + str(linear_y)) self.driveControl.publish(t) self.last_twist = t @@ -274,6 +258,7 @@ def run(self): cur = self.OdomLocation.get_pose() self.Start = cur self.Goal = Pose2D() + goal_dist = math.inf if req_theta == 0: self.Doing.request.linear = State.DRIVE_SPEED if self.Doing.request.y == 0: # Forward Backwards drive @@ -306,7 +291,8 @@ def run(self): self.Doing.request.angular = State.TURN_SPEED_SLOW # CLOSE so slowing down if math.isclose(heading_error, 0, abs_tol=0.1): self.Doing.request.angular = State.TURN_SPEED_SLOW / 4 # SOOO CLOSE so slowing down more - if abs(heading_error) > State.ROTATE_THRESHOLD and not math.isclose(heading_error, 0, abs_tol=0.001): + if abs(heading_error) > State.ROTATE_THRESHOLD and not math.isclose(heading_error, 0, + abs_tol=0.001): if heading_error < 0: self.drive(0, 0, -self.Doing.request.angular, State.DRIVE_MODE_PID) else: @@ -315,8 +301,6 @@ def run(self): self.CurrentState = State.STATE_IDLE self.drive(0, 0, 0, State.DRIVE_MODE_STOP) elif self.CurrentState == State.STATE_DRIVE: - direction_x = 1 - direction_y = 1 if self.Doing.request.y != 0: print_debug('STRAFE') else: @@ -330,50 +314,64 @@ def run(self): heading_error = angles.shortest_angular_distance(cur.theta, self.Goal.theta) goal_angle = angles.shortest_angular_distance(math.pi + cur.theta, math.atan2(self.Goal.y - cur.y, self.Goal.x - cur.x)) - print_debug("dist: " + str(math.hypot(self.Goal.x - self.OdomLocation.Odometry.pose.pose.position.x, - self.Goal.y - self.OdomLocation.Odometry.pose.pose.position.y))) - if self.OdomLocation.at_goal(self.Goal, State.GOAL_DISTANCE_OK): - print_debug('at goal') - self.Goal = None - self.CurrentState = State.STATE_IDLE - self.drive(0, 0, 0, State.DRIVE_MODE_STOP) - # elif abs(goal_angle) > State.DRIVE_ANGLE_ABORT: - # @TODO NO OVER SHOT CODE - - """ - if self.OdomLocation.at_goal(self.Goal, State.GOAL_DISTANCE_OK) or (self.Doing.request.linear_y == 0 and - abs(goal_angle) > - State.DRIVE_ANGLE_ABORT): - if abs(goal_angle) > State.DRIVE_ANGLE_ABORT: - print_debug('goal_angle exceeded ABORT') + new_goal_dist = math.hypot(self.Goal.x - self.OdomLocation.Odometry.pose.pose.position.x, + self.Goal.y - self.OdomLocation.Odometry.pose.pose.position.y) + if goal_dist - new_goal_dist < -State.GOAL_DISTANCE_OK: # overshot allowed distance + print_debug('overshot: ' + str(new_goal_dist)) + self.drive(0, 0, 0, + State.DRIVE_MODE_PID) # for waiting use DRIVE_MODE_PID so we dont clear the task + r.sleep() + new_goal_dist = math.hypot(self.Goal.x - self.OdomLocation.Odometry.pose.pose.position.x, + self.Goal.y - self.OdomLocation.Odometry.pose.pose.position.y) + if new_goal_dist <= State.GOAL_DISTANCE_OK: # stopped and double check + print_debug('at goal') self.Goal = None self.CurrentState = State.STATE_IDLE self.drive(0, 0, 0, State.DRIVE_MODE_STOP) - - elif abs(heading_error) > State.DRIVE_ANGLE_ABORT / 2: - self._stop_now(MoveResult.PATH_FAIL) - self.drive(0, 0, 0, State.DRIVE_MODE_STOP) - --------------- - - elif abs(heading_error) > State.DRIVE_ANGLE_ABORT / 2: - print_debug('heading_error exceeded') + else: + print_debug('overshot: stopping @TODO FIX ME') + self.Goal = None self.CurrentState = State.STATE_IDLE - self._stop_now(MoveResult.PATH_FAIL) self.drive(0, 0, 0, State.DRIVE_MODE_STOP) - """ + """ @TODO Fix & finish this code so if overshoot can set a new goal or if exceeds PATH_FAIL + if not math.isclose(self.Goal.x - self.OdomLocation.Odometry.pose.pose.position.x, 0, + abs_tol=State.GOAL_DISTANCE_OK): + # if self.Goal.x - self.OdomLocation.Odometry.pose.pose.position.x > 0: + self.Doing.request.x = self.Goal.x - self.OdomLocation.Odometry.pose.pose.position.x + self.Doing.request.linear = State.DRIVE_SPEED_SLOW # State.DRIVE_SPEED / 4 + print_debug('overshot recalc x' + str(self.Doing.request.x)) + if not math.isclose(self.Goal.y - self.OdomLocation.Odometry.pose.pose.position.y, 0, + abs_tol=State.GOAL_DISTANCE_OK): + self.Doing.request.y = self.Goal.y - self.OdomLocation.Odometry.pose.pose.position.y + self.Doing.request.linear = State.DRIVE_SPEED_SLOW # State.DRIVE_SPEED / 4 + print_debug('overshot recalc y' + str(self.Doing.request.y)) + """ + elif self.OdomLocation.at_goal(self.Goal, State.GOAL_DISTANCE_OK): + print_debug('at goal') + self.Goal = None + self.CurrentState = State.STATE_IDLE + self.drive(0, 0, 0, State.DRIVE_MODE_STOP) + elif self.Doing.request.y == 0 and abs(heading_error) > State.DRIVE_ANGLE_ABORT / 2: + print_debug('heading_error exceeded') + self.Goal = None + self.Doing.result = MoveResult.PATH_FAIL + self.CurrentState = State.STATE_IDLE + self._stop_now(MoveResult.PATH_FAIL) + self.drive(0, 0, 0, State.DRIVE_MODE_STOP) else: if abs(self.Doing.request.y) + abs(self.Doing.request.x) == 0: linear_x = 0 else: - linear_x = self.Doing.request.linear * direction_x * self.Doing.request.x / ( + linear_x = self.Doing.request.linear * self.Doing.request.x / ( abs(self.Doing.request.y) + abs(self.Doing.request.x)) if abs(self.Doing.request.y) + abs(self.Doing.request.x) == 0: linear_y = 0 else: - linear_y = self.Doing.request.linear * direction_y * self.Doing.request.y / ( + linear_y = self.Doing.request.linear * self.Doing.request.y / ( abs(self.Doing.request.y) + abs(self.Doing.request.x)) self.drive(linear_x, linear_y, heading_error * State.HEADING_RESTORE_FACTOR, State.DRIVE_MODE_PID) + goal_dist = new_goal_dist elif self.CurrentState == State.STATE_TIMED: print_debug('TIMED') From a13cd605f18e53389e33c73d4f74572f34878cf3 Mon Sep 17 00:00:00 2001 From: Carter Frost Date: Wed, 28 Apr 2021 12:17:57 -0700 Subject: [PATCH 18/18] src/scoot/src/Scoot.py: remove duplicate function lock --- src/scoot/src/Scoot.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/scoot/src/Scoot.py b/src/scoot/src/Scoot.py index 41ee509..1bd4bdc 100755 --- a/src/scoot/src/Scoot.py +++ b/src/scoot/src/Scoot.py @@ -284,8 +284,7 @@ def get_joint_pos(self, joint_name): @Sync(odom_lock) def get_odom_location(self): - with odom_lock: - return self.OdomLocation + return self.OdomLocation @Sync(odom_lock) def get_true_pose(self):