def StuckMonitor(self):
self.blacklistUpdated = False
if self.agent.goal.path.poses and len(self.history) == self.hislen:
# Check if we've been stopped if we have a goal
if (getDist(self.history[0].position, self.history[-1].position) <
0.5 and abs(
angleDiff(
math.degrees(getYaw(self.history[0].orientation)),
math.degrees(getYaw(
self.history[-1].orientation)))) < 60):
self.stuck += 1
# Add the current goal to potential blacklist points
self.blgoals.append(self.agent.goal.pose.pose.position)
else:
self.stuck = 0
self.blgoals = []
# If stuck, report and append to blacklist so we don't try to go here again
if self.stuck >= self.stopCheck:
# Only add to the blacklist at the stopCheck intervals,
# or else they get added too often
# If in GUI Goal, just exit back to explore
if self.guiBehavior == 'goToGoal':
self.guiBehavior = None
elif self.stuck % self.stopCheck == 0:
# Get the average goal position
avgGoal = averagePosition(self.blgoals)
# Remove outliers
newgoals = []
for goal in self.blgoals:
if getDist(goal, avgGoal) < self.deconflictRadius * 2:
newgoals.append(goal)
# Get the new average goal position, and only if there are enough
if len(newgoals) > self.hislen / 2:
avgGoal = averagePosition(newgoals)
# Make sure it's not the origin and we're not already close to the goal
if (not (avgGoal.x == 0 and avgGoal.y == 0)
and getDist(
self.agent.odometry.pose.pose.position,
self.agent.goal.pose.pose.position) > 0.5):
self.addBlacklist(avgGoal)
self.blgoals = []
# Tell the planner to replan and blacklist
self.blacklistUpdated = True
self.updateStatus('Stuck')
rospy.loginfo(self.id + ' has not moved!')
elif not self.agent.goal.path.poses:
# Report no path available
self.updateStatus('No Path')
def checkBlacklist(self, goal):
addBlacklist = True
# Make sure it's not already in a blacklist radius
for bgoal in self.blacklist.points:
if getDist(bgoal, goal) < self.deconflictRadius:
addBlacklist = False
return addBlacklist
def execute(self):
# This part should probably be in a Monitor instead
if (getDist(self.a.agent.odometry.pose.pose.position,
self.a.agent.guiGoalPoint.pose.position) < 1.0):
rospy.loginfo(self.a.id + ' resuming exploration...')
self.a.guiBehavior = None
# May want to add other options for tasks when it reaches the goal
self.a.behaviors['explore'].execute()
else:
if self.a.agent.status != 'guiCommand':
rospy.loginfo(self.a.id + ' setting GUI Goal Point...')
self.a.setGoalPoint('guiCommand')
def updateHistory(self):
# Check whether we've started the mission by moving 3 meters
if not self.initialPose:
# Make sure we have a valid pose and it's not just the blank initialized pose
if (self.agent.odometry.pose.pose.position.x != 0
and self.agent.odometry.pose.pose.position.y != 0
and self.agent.odometry.pose.pose.position.z != 0):
self.initialPose = self.agent.odometry.pose.pose
elif not self.startedMission:
# Do this here so we only do this calculation until leaving the starting area
if getDist(self.agent.odometry.pose.pose.position,
self.initialPose.position) > 3:
rospy.loginfo('Started Mission')
self.startedMission = True
self.history.append(self.agent.odometry.pose.pose)
if len(self.history) > self.hislen:
self.history = self.history[-self.hislen:]
def beaconDistCheck(self, pose, checkDist, dropBeacon):
numBeacons = 0
numDistBeacons = 0
for beacon in self.beacons.values():
if beacon.active:
# Count the beacons we know about, and check distance
numBeacons = numBeacons + 1
dist = getDist(pose.position, beacon.pos)
# Count how many beacons are past max range
if dist > checkDist:
numDistBeacons = numDistBeacons + 1
dropBeacon = True
# Cancel the drop if we have more than one beacon in range
if numBeacons - numDistBeacons > 0:
dropBeacon = False
return dropBeacon, numBeacons, numDistBeacons
def execute(self):
# Explore with goal deconfliction
self.a.stopStart = True
# Reduce all the redundant explore messages
if self.a.agent.status != 'Explore':
self.a.move()
self.a.agent.status = 'Explore'
self.a.home_pub.publish(False)
self.a.task_pub.publish(self.a.agent.status)
# Find the best goal point to go to, unless we need a blacklist replan
if not self.a.blacklistUpdated:
self.a.deconflictGoals()
# If a replan was requested somewhere, trigger it, unless we already asked recently
alreadyReplanned = False
if ((self.a.lastReplanTime + rospy.Duration(self.a.stopCheck) >
rospy.get_rostime()
or self.a.lastReplanGoal != self.a.agent.exploreGoal)
and (self.a.replan or self.a.blacklistUpdated)):
alreadyReplanned = True
self.a.lastReplanGoal = self.a.agent.exploreGoal
self.a.lastReplanTime = rospy.get_rostime()
self.a.updateStatus('Replanning')
rospy.loginfo(self.a.id + ' requesting replan')
if self.a.blacklistUpdated:
self.a.task_pub.publish('unstuck')
else:
self.a.task_pub.publish(self.a.replanCommand)
self.a.replan = False
# If the planner can't plan, and we've reached the previous goal, or are stuck,
# switch to trajectory follower to go towards home
if self.a.useTraj or (
not self.a.planner_status and
(self.a.stuck > self.a.stopCheck
or getDist(self.a.agent.goal.pose.pose.position,
self.a.agent.odometry.pose.pose.position) < 1.0)):
# Try to get the planner to replan
if not alreadyReplanned:
self.a.task_pub.publish(self.a.replanCommand)
if self.a.useExtTraj:
self.a.updateStatus('Following Trajectory')
rospy.loginfo(self.a.id +
' using trajectory follower during explore')
self.a.traj_pub.publish(True)
# Stop using the old goal and path or else we'll get stuck in a loop
self.a.agent.goal.pose = self.a.agent.exploreGoal
self.a.agent.goal.path = self.a.agent.explorePath
# If we're stuck, with no plan, but we've reached the end of the path, blacklist this
if len(self.a.agent.goal.path.poses) > 0:
pathend = self.a.agent.goal.path.poses[-1].pose.position
if (not self.a.planner_status and getDist(
pathend, self.a.agent.odometry.pose.pose.position) <
1.0):
self.a.addBlacklist(pathend)
elif self.a.useExtTraj:
self.a.traj_pub.publish(False)
# Publish the selected goal and path for the guidance controller
self.a.goal_pub.publish(self.a.agent.goal.pose)
if self.a.agent.goal.path.header.frame_id != 'starting':
self.a.path_pub.publish(self.a.agent.goal.path)
def BeaconMonitor(self):
self.deployBeacon = False
# Check if we need to drop a beacon if we have any beacons to drop
if self.numBeacons > 0:
pose = self.agent.odometry.pose.pose
dropBeacon = False
dropReason = ''
# We're connected to the mesh, either through anchor or beacon(s)
if self.base.incomm:
# Once we pass the maxDist we could set a flag so we don't keep recalculating this
anchorDist = getDist(pose.position, self.anchorPos)
# Beacon distance based drop only kicks in once out of anchor range
checkDist = self.maxAnchorDist
# If we're too close (like for the initial node drop), never drop a beacon
if anchorDist < self.minAnchorDist:
return
# If we're at end of anchor range, drop beacon
if anchorDist > self.maxAnchorDist and not self.delayDrop:
dropBeacon = True
dropReason = 'anchor distance'
checkDist = self.maxDist
# Always drop a beacon if we're at a node and we're in comm
# If beacons are strong enough may want to restrict distance
if self.agent.atnode.data:
dropBeacon = True
dropReason = 'at junction'
checkDist = self.junctionDist
# Check if it looks like we're going around a corner
elif self.turnDetect and len(self.history) == self.hislen:
pos1, yaw1 = averagePose(self.history[:int(0.4 *
self.hislen)])
pos2, yaw2 = averagePose(self.history[int(0.6 *
self.hislen):])
# Check that we've turned and moved far enough, over history and last second
# Will need to retune these for real vehicle dynamics
if (getDist(pos1, pos2) > 4
and abs(angleDiff(yaw1, yaw2)) > 30
and getDist(self.history[-2].position,
self.history[-1].position) > 0.5):
dropBeacon = True
dropReason = 'at turn'
checkDist = self.junctionDist + 5
dropBeacon, numBeacons, numDistBeacons = self.beaconDistCheck(
pose, checkDist, dropBeacon)
if numDistBeacons > 0 and (dropReason == ''
or dropReason == 'anchor distance'):
dropReason = 'beacon distance'
# Prevent dropping after returning home after the first beacon drop
# TODO look at the angle between anchor and first beacon and calculate positions
# This only works for straight departure now
if numBeacons > 0 and anchorDist < self.maxAnchorDist and pose.position.y < 1 and pose.position.y > 1:
dropBeacon = False
if dropBeacon:
if self.delayDrop:
self.delayDrop = False
else:
# Set the monitor so the deployment should get executed
self.dropReason = dropReason
self.deployBeacon = True
def ReverseDropMonitor(self):
if not self.numBeacons:
return
# Reset the ability to check for reverse drop if we ever regain comms
if self.base.incomm:
self.checkReverse = True
self.beaconCommLost = 0
# Check for reverse if we haven't disabled due to a blacklist, if not in comms,
# and if we're not currently trying to reverse drop
if self.checkReverse and not self.base.incomm and not self.reverseDrop:
self.beaconCommLost += 1
# If we're not talking to the base station, attempt to reverse drop
if self.beaconCommLost > 5:
self.reverseDrop = True
# Check if we've already attempted a reverse drop in this area
# Sometimes there are deadzones and this can cause a loop if not accounted for
pose = self.agent.odometry.pose.pose
for bl in self.bl_beacons:
if getDist(pose.position, bl) < self.junctionDist:
self.reverseDrop = False
# Disable checking again unless we regain comms at some point
self.checkReverse = False
rospy.loginfo(
self.id +
' skipping reverse drop due to previous try')
break
# Add to the list of previously tried positions
if self.reverseDrop:
self.bl_beacons.append(pose.position)
# Reset the counter so we don't attempt again right away
self.beaconCommLost = 0
# If we've regained comms and were trying to reverse drop, check if we can
if self.reverseDrop and self.base.incomm:
pose = self.agent.odometry.pose.pose
checkDist = self.junctionDist
if self.regainBase > 5:
# Make sure there's no beacons already in the area
dropBeacon = True
dropBeacon, nB, nDB = self.beaconDistCheck(
pose, checkDist, dropBeacon)
if dropBeacon:
self.dropReason = 'Regain comms'
self.deployBeacon = True
else:
# The bl_beacons should prevent this from occuring too many times
rospy.loginfo(self.id +
' beacon too close, cancelling drop')
self.regainBase = 0
self.reverseDrop = False
else:
anchorDist = getDist(pose.position, self.anchorPos)
if anchorDist > 10:
self.regainBase += 1
else:
# If we're close to anchor, comms must just be too bad to keep trying to drop
# Drop one anyway if there's not already one nearby just in case it helps
dropBeacon = True
dropBeacon, nB, nDB = self.beaconDistCheck(
pose, checkDist, dropBeacon)
if dropBeacon:
self.dropReason = 'Near anchor'
self.deployBeacon = True
else:
rospy.loginfo(self.id +
' anchor too close, cancelling drop')
self.regainBase = 0
self.reverseDrop = False
else:
self.regainBase = 0
def deconflictGoals(self):
# Get all of the goals into a list
goals = self.agent.goals.goals
# Last goal and path chosen
curgoal = self.agent.goal.pose.pose.position
curpos = self.agent.odometry.pose.pose.position
expgoal = self.agent.exploreGoal.pose.position
# Keep going to long distance goal, unless it's home, or we get a new potential goal close
if (getDist(curpos, expgoal) > self.mappingRange
and getDist(curpos, curgoal) > self.mappingRange + 2.0 and
not (curgoal.x == 0 and curgoal.y == 0 and curgoal.z == 0)):
# Make sure this goal isn't blacklisted, which could happen after it was chosen
conflict = False
for goal in self.blacklist.points:
if getDist(curgoal, goal) < self.deconflictRadius:
conflict = True
break
if not conflict:
# Check if there's an updated path for this goal, or one near it
if not goals:
if getDist(curgoal,
self.agent.exploreGoal.pose.position) < 0.5:
self.agent.goal.pose = self.agent.exploreGoal
self.agent.goal.path = self.agent.explorePath
else:
for goal in goals:
if getDist(curgoal, goal.pose.pose.position) < 0.5:
self.agent.goal = goal
break
rospy.loginfo(self.id + ' continuing to long distance goal')
return
if (self.singleGoalDeconflict and
not (expgoal.x == 0 and expgoal.y == 0 and expgoal.z == 0)):
# Only getting one goal point so have to request a replan if there's a conflict
gpos = self.agent.exploreGoal.pose.position
godom = self.agent.odometry.pose.pose.position
# Assume the point will be good to start
conflict = False
# Global planner should take care of this now, but it's a double check
for goal in self.blacklist.points:
if getDist(gpos, goal) < self.deconflictRadius:
conflict = True
self.updateStatus('Replanning Blacklist')
rospy.loginfo(self.id + ' replanning due to blacklist')
break
if not conflict:
# Check each neighbors' goal for conflict
for neighbor in self.neighbors.values():
npos = neighbor.goal.pose.pose.position
nodom = neighbor.odometry.pose.pose.position
# Check whether they are within the defined range
if getDist(gpos, npos) < self.deconflictRadius:
# If our cost is more than the neighbor, don't go to this goal
if getDist(gpos, godom) > getDist(npos, nodom):
conflict = True
self.updateStatus('Replanning Neighbor')
rospy.loginfo(self.id +
' replanning due to neighbor')
# Don't need to check any more neighbors for this goal if conflict
break
if conflict:
# Request a replan
self.replan = True
# Set the goal to the planner's goal
self.agent.goal.pose = self.agent.exploreGoal
self.agent.goal.path = self.agent.explorePath
elif not goals:
# Set the goal to the frontier goal
# This should cause us to navigate to the goal, then go home if still no plan
self.agent.goal.pose = self.agent.exploreGoal
self.agent.goal.path = self.agent.explorePath
elif len(goals) == 1:
# If we only have one potential goal, just go there
# May want to consider stopping in place if there is a conflict!
self.agent.goal = goals[0]
else:
# Receiving multiple goals from planner, so we can choose one directly
# TODO add check for location of neighbor and don't go there
# Otherwise, deconflict with neighbor goals
# Start true to initiate loop
conflict = True
i = 0
goodGoal = None
while conflict and i < len(goals):
# Check each goal in order for conflict with any neighbors
gpos = goals[i].pose.pose.position
# Assume the point will be good to start
conflict = False
# Global planner should take care of this now, but it's a double check
for goal in self.blacklist.points:
if getDist(gpos, goal) < self.deconflictRadius:
conflict = True
self.updateStatus('Replanning Blacklist')
rospy.loginfo(self.id +
' replanning due to blacklist ' + str(i))
break
if not conflict:
# If it's not blacklisted, identify the 'best' (first) goal
if not goodGoal:
goodGoal = goals[i]
# Check each neighbors' goal for conflict
for neighbor in self.neighbors.values():
npos = neighbor.goal.pose.pose.position
# Check whether they are within the defined range
if getDist(gpos, npos) < self.deconflictRadius:
# If our cost is more than the neighbor, don't go to this goal
if goals[i].cost.data > neighbor.goal.cost.data:
conflict = True
self.updateStatus('Replanning Neighbor')
rospy.loginfo(self.id +
' replanning due to neighbor ' +
str(i))
# Don't need to check any more neighbors for this goal if conflict
break
# Check the next goal
i += 1
# Decide which goal to use, or whether to go home
self.useTraj = False
if conflict:
# Conflict will still be true if all goals conflict, so use the best
if goodGoal:
self.agent.goal = goodGoal
if len(goals) > 1:
rospy.loginfo(self.id +
' all goals conflict, using best')
else:
# All goals blacklisted, start going home, but clear the blacklist
self.agent.goal.pose = self.agent.exploreGoal
self.agent.goal.path = self.agent.explorePath
self.blacklist.points = []
rospy.loginfo(self.id + ' all goals blacklisted ' +
str(len(goals)))
self.useTraj = True
else:
# Set the goal to the last goal without conflict
self.agent.goal = goals[i - 1]
# Final blacklist check for single goal
if not self.singleGoalDeconflict and (not goals or len(goals) == 1):
goal = self.agent.goal.pose.pose.position
if len(self.agent.goal.path.poses) > 0:
pathend = self.agent.goal.path.poses[-1].pose.position
else:
pathend = goal
self.useTraj = False
if not self.checkBlacklist(goal) or not self.checkBlacklist(
pathend):
rospy.loginfo(
self.id +
' only goal is blacklisted, using trajectory follower')
self.useTraj = True
def artifactCheck(self, agent):
updateString = False
# Check the artifact list received from the artifact manager for new artifacts
for artifact in agent.newArtifacts.artifacts:
aid = artifact.artifact_id
if aid not in self.artifacts and artifact.position.x and artifact.position.y:
addArtifact = True
# Check if there's a similar artifact already stored so we don't report
if agent.id == self.id:
# Mark that we need to update our hash
updateString = False
ignore = False
for artifact2 in self.artifacts.values():
if getDist2D(artifact.position,
artifact2.artifact.position) < 3:
ignore = True
# Skip artifacts that might be another robot
if artifact.obj_class == 'rope':
for neighbor in self.neighbors.values():
if getDist(neighbor.odometry.pose.pose.position,
artifact.position) < 5:
addArtifact = False
ignore = True
rospy.loginfo(
self.id +
' skipping artifact due to neighbor')
break
for beacon in self.beacons.values():
if getDist(beacon.pos, artifact.position) < 2:
addArtifact = False
ignore = True
rospy.loginfo(
self.id +
' skipping artifact due to beacon')
break
if not ignore:
self.report = True
updateString = True
# Now add the artifact to the array
self.artifacts[aid] = ArtifactReport(agent.id, artifact,
self.sendImages)
if addArtifact:
agent.addArtifact(artifact)
else:
self.artifacts[aid].reported = True
# At the base station, fuse the artifacts and make sure we mark an update
if self.type == 'base':
updateString = True
self.fuseArtifact(self.artifacts[aid])
rospy.loginfo(self.id + ' new artifact from ' + agent.id +
' ' + artifact.obj_class + ' ' + aid)
if updateString:
agent.updateHash()
return True
return False
