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