Skip to content

Commit

Permalink
fix: Newlines before comments, removed extraneous whitespace
Browse files Browse the repository at this point in the history
  • Loading branch information
figuernd committed Mar 12, 2024
1 parent 2f5cf7b commit 86d2f9c
Show file tree
Hide file tree
Showing 9 changed files with 35 additions and 8 deletions.
1 change: 1 addition & 0 deletions scripts/config_validation.py
Original file line number Diff line number Diff line change
Expand Up @@ -65,6 +65,7 @@ def validate_config_recursive(user_data, default_data, annotations, key_path=[])
elif isinstance(default_value, dict):
if not validate_config_recursive(user_data[key], default_value, annotations, current_path):
all_valid = False

# If a key from default config is not in the annotations, it's assumed required.
elif key not in user_data:
print(f"Key {'.'.join(current_path)} is required since not #optional but missing from the user YAML")
Expand Down
10 changes: 5 additions & 5 deletions scripts/tmux_run.sh
Original file line number Diff line number Diff line change
Expand Up @@ -16,29 +16,29 @@ if tmux has-session -t phyto-arm 2>/dev/null; then
else
# Create the session and name the window
tmux new-session -d -s phyto-arm -n docker

# Split the window into three panes
tmux split-window -v
tmux split-window -v
tmux select-layout even-vertical

# Select pane 1 and launch the main process in docker
tmux select-pane -t 0
tmux send-keys "./scripts/docker_run.sh -b $CONFIG" C-m
tmux send-keys "./phyto-arm start main mounted_config.yaml" C-m

# Select pane 2 and launch the IFCB arm in the same container
tmux select-pane -t 1
tmux send-keys "sleep 8" C-m
tmux send-keys "docker exec -it phyto-arm bash" C-m
tmux send-keys "./phyto-arm start arm_ifcb ./mounted_config.yaml" C-m

# Select pane 3 and launch the Chanos arm in the same container
tmux select-pane -t 2
tmux send-keys "sleep 10" C-m
tmux send-keys "docker exec -it phyto-arm bash" C-m
tmux send-keys "./phyto-arm start arm_chanos ./mounted_config.yaml" C-m

# Attach to the session
tmux attach -t phyto-arm
fi
2 changes: 1 addition & 1 deletion src/jvl_motor/src/jvl_motor_node.py
Original file line number Diff line number Diff line change
Expand Up @@ -148,7 +148,7 @@ def main():
msg.Motion,
queue_size=1
)

# Create services
services = [
rospy.Service(
Expand Down
11 changes: 10 additions & 1 deletion src/phyto_arm/src/arm_base.py
Original file line number Diff line number Diff line change
Expand Up @@ -45,6 +45,7 @@ def __init__(self, arm_name, winch_name=None):
acquire_move_clearance = rospy.ServiceProxy('/lock_manager/acquire', LockOperation)
release_move_clearance = rospy.ServiceProxy('/lock_manager/release', LockOperation)
lock_op = LockOperationRequest(arm_name)

# Bind lock operations to instance
self.request_clearance = lambda: acquire_move_clearance(lock_op).success
self.release_clearance = lambda: release_move_clearance(lock_op).success
Expand All @@ -54,6 +55,7 @@ def __init__(self, arm_name, winch_name=None):
rospy.loginfo('Awaiting lock manager check...')
check_lock.wait_for_service()
rospy.loginfo('Server acquired for lock check.')

# Check whether we have a dangling acquisition to release
if check_lock(LockCheckRequest(arm_name)).has_lock:
rospy.logwarn('Releasing dangling lock acquisition')
Expand All @@ -76,31 +78,35 @@ def winch_busy(self):
def send_winch_goal(self, depth, speed, callback):
global winches_moving
rospy.loginfo(f"Sending winch move to {depth}")

# Ensure winch is enabled
if rospy.get_param('winch/enabled') != True:
raise ValueError('Move aborted: winch is disabled in config')

# Safety check: Do not exceed depth bounds
if depth < rospy.get_param('winch/range/min'):
raise ValueError(f'Move aborted: depth {depth} is below min {rospy.get_param("winch/range/max")}')
elif depth > rospy.get_param('winch/range/max'):
raise ValueError(f'Move aborted: depth {depth} is above max {rospy.get_param("winch/range/max")}')

# Set max speed if not specified
if speed is None:
speed = rospy.get_param('winch/max_speed')

# Safety check: Speed cannot exceed max speed
elif speed > rospy.get_param('winch/max_speed'):
raise ValueError(f'Move aborted: speed {speed} is above max {rospy.get_param("winch/max_speed")}')
if self.winch_busy():
raise RuntimeError('Move aborted: winch in unexpected busy state')
rospy.loginfo(f"All checks passed; Moving winch to {depth}")


# Intermediate callback for ensuring move was a success and to release lock
# before calling the task's callback
def winch_done(state, move_result):
try:
# Ensure winch move was successful
assert state == actionlib.GoalStatus.SUCCEEDED

# Free up semaphore for another winch movement
assert self.release_clearance()
callback(move_result)
Expand Down Expand Up @@ -134,15 +140,18 @@ def loop(self):
task = self.get_next_task(task)
rospy.logwarn(f'No winch; running {task.name}')
task.callback()

# Otherwise, move winch
else:
rospy.logwarn('Arm waiting for clearance')

# TODO: Consider replacing this with a queueing mechanism. Requires setting up
# callbacks via ROS service calls. Unnecessarily complex for a 2 winch system,
# but might be the only way to achieve round-robin for many winches.
while not rospy.is_shutdown() and not self.request_clearance():
# Wait until central semaphore clears us to move
rospy.sleep(1)

# TODO: Optimize task evaluation. Currently we are blocking on the assumption that
# movement will be needed; this is not always the case. Not a problem for 1 or 2
# winches, but could get slow if the number of winches greatly exceeds the
Expand Down
1 change: 1 addition & 0 deletions src/phyto_arm/src/arm_chanos.py
Original file line number Diff line number Diff line change
Expand Up @@ -112,6 +112,7 @@ def compute_profiler_steps():

def on_profile_msg(msg):
arm.latest_profile = msg

# Find the maximal value in the profile
argmax = max(range(len(arm.latest_profile.values)), key=lambda i: arm.latest_profile.values[i])
arm.profiler_peak_value = arm.latest_profile.values[argmax]
Expand Down
11 changes: 11 additions & 0 deletions src/phyto_arm/src/arm_ifcb.py
Original file line number Diff line number Diff line change
Expand Up @@ -63,6 +63,7 @@ def get_next_task(self, last_task):

# Global reference to action provided by other node
ifcb_runner = None

# Global reference to arm state
arm = None

Expand All @@ -79,24 +80,29 @@ def on_profile_msg(msg):
def wiz_to_rostime(hhmm_time):
# Today's time
today_time = datetime.now().time()

# Convert HH:MM to a timestamp
wiz_time = datetime.strptime(hhmm_time, "%H:%M").time()
wiz_date = datetime.now().date()

# wiz_time should always come after the current time, even if it's the next day
if wiz_time < today_time:
wiz_date = datetime.now().date() + timedelta(days=1)
unix_timestamp = datetime.combine(wiz_date, wiz_time).timestamp()

# Create a rospy.Time object
return rospy.Time.from_sec(unix_timestamp)


def find_next_wiz_time():
# Get array of HH:MM times
wiz_probe_times = rospy.get_param('tasks/wiz_probe/times')

# Find the next time
next_time = None
for wiz_time in wiz_probe_times:
wiz_time = wiz_to_rostime(wiz_time)

# wiz_time will always be in the future, just find the earliest one
if next_time is None or wiz_time < next_time:
next_time = wiz_time
Expand All @@ -106,6 +112,7 @@ def find_next_wiz_time():
# Check if it's time to run the wiz probe routine
def its_wiz_time():
prep_window = rospy.Duration(60*rospy.get_param('tasks/wiz_probe/preparation_window'))

# If we're within the preparation window, it's wizin' time
if (find_next_wiz_time() - rospy.Time.now()) <= prep_window:
return True
Expand Down Expand Up @@ -135,16 +142,19 @@ def compute_wiz_depth(peak_depth):
def await_wiz_probe(callback):
# Find the next time to run the wiz probe
next_time = find_next_wiz_time()

# Ensure we're still in the preparation window
prep_window = rospy.Duration(60*rospy.get_param('tasks/wiz_probe/preparation_window'))
preptime_remaining = next_time - rospy.Time.now()
if preptime_remaining > prep_window:
raise ValueError(f'Next wiz time is {preptime_remaining.to_sec()}s away, should be less than \
{prep_window.to_sec()}s. Preparation window likely too short.')

# Wait for the rest of the window + the wiz probe duration
rospy.loginfo('Waiting for wiz probe to finish')
wiz_duration = 60*rospy.get_param('tasks/wiz_probe/duration')
rospy.sleep(wiz_duration + preptime_remaining.to_sec())

# TODO: Run IFCB while waiting
rospy.loginfo('Wiz probe wait time complete')
callback()
Expand Down Expand Up @@ -193,6 +203,7 @@ def handle_downcast(move_result):
while arm.latest_profile is None or \
arm.latest_profile.goal_uuid != move_result.uuid:
notified = arm.profile_activity.wait(60)

# For short downcasts profiling will likely fail; switch to scheduled depth
if not notified:
rospy.logwarn('No profile data received')
Expand Down
3 changes: 3 additions & 0 deletions src/phyto_arm/src/mocks/mock_ctd.py
Original file line number Diff line number Diff line change
Expand Up @@ -9,11 +9,13 @@

def generate_mock_ctd():
ctd = Ctd()

# Generate random or fixed values for CTD parameters
ctd.conductivity = random.uniform(0, 5) # Example random value, in S/m
ctd.temperature = random.uniform(-2, 30) # Example random value, in Celsius
ctd.pressure = random.uniform(0, 600) # Example random value, in dbar
ctd.sound_speed = random.uniform(1400, 1600) # Example random value, in m/s

# Set covariance fields to -1, indicating "not valid"
ctd.conductivity_covar = ctd.temperature_covar = ctd.pressure_covar = \
ctd.salinity_covar = ctd.sound_speed_covar = -1
Expand All @@ -36,6 +38,7 @@ def main():
global depth_pub
rospy.init_node('mock_sensor_node')
rospy.logwarn(f'Starting mock sensor node {rospy.get_name()}')

# Subscribe to the mock depth topic from the 'mock_winch'
rospy.Subscriber("mock_depth", Float64, depth_callback)

Expand Down
2 changes: 2 additions & 0 deletions src/phyto_arm/src/winch_node.py
Original file line number Diff line number Diff line change
Expand Up @@ -134,6 +134,7 @@ async def move_to_depth(server, goal):
asyncio.create_task(depth.wait()): "depth status",
asyncio.create_task(motor.wait()): "motor status"
}

# Get an initial depth fix and motor status message
_, pending = await asyncio.wait(tasks.keys(), timeout=2.0, return_when=asyncio.ALL_COMPLETED)

Expand Down Expand Up @@ -169,6 +170,7 @@ async def move_to_depth(server, goal):
# Velocity function
max_speed = rospy.get_param('~max_speed' )
half_speed_dist = rospy.get_param('~half_speed_dist')

# Assert both are not None
assert max_speed is not None and half_speed_dist is not None, 'Winch speed config invalid'
assert goal.velocity < max_speed, 'Goal velocity exceeds max speed'
Expand Down
2 changes: 1 addition & 1 deletion src/rbr_maestro3_ctd/src/rbr_maestro3_node.py
Original file line number Diff line number Diff line change
Expand Up @@ -99,7 +99,7 @@ async def main():
# Set covariance fields to -1, the standard "not valid" value
ctd.conductivity_covar = ctd.temperature_covar = ctd.pressure_covar = \
ctd.salinity_covar = ctd.sound_speed_covar = -1

# Construct the DepthPressure message
dp = DepthPressure()
dp.depth = depth
Expand Down

0 comments on commit 86d2f9c

Please sign in to comment.