def create_move_base_goal(p):
target = geometry_msgs.PoseStamped()
target.header.frame_id = p['frame_id']
target.header.stamp = rospy.Time.now()
target.pose = create_geo_pose(p)
goal = move_base_msgs.MoveBaseGoal(target)
return goal
def execute(self, userdata):
print 'waiting for transform'
self.tf_listener.waitForTransform(self.pose_stamped.header.frame_id,
'/map', rospy.Time(0),
rospy.Duration(10.))
self.pose_stamped.header.stamp = rospy.Time()
ps = self.tf_listener.transformPose('/map', self.pose_stamped)
g = mm.MoveBaseGoal()
p = g.target_pose
p.header.frame_id = ps.header.frame_id
p.header.stamp = rospy.get_rostime()
p.pose.position.x = ps.pose.position.x
p.pose.position.y = ps.pose.position.y
p.pose.position.z = 0 #ps.pose.position.z
raw_angles = tr.euler_from_quaternion([
ps.pose.orientation.x, ps.pose.orientation.y,
ps.pose.orientation.z, ps.pose.orientation.w
])
quat = tr.quaternion_from_euler(0., 0., raw_angles[2])
p.pose.orientation.x = quat[0]
p.pose.orientation.y = quat[1]
p.pose.orientation.z = quat[2]
p.pose.orientation.w = quat[3]
print g
self.move_base_client.send_goal(g)
result = tu.monitor_goals(self, [self.move_base_client],
'NavigateSmach', 60 * 60)
if result == 'failed':
return 'aborted'
return result
def move_to(self, x, y, theta):
g = mbm.MoveBaseGoal()
g.target_pose.header.frame_id = 'odom_combined'
g.target_pose.header.stamp = rospy.Time.now()
g.target_pose.pose.position = gm.Point(x, y, 0)
q = tr.quaternion_from_euler(0, 0, theta)
g.target_pose.pose.orientation = gm.Quaternion(*q)
loginfo("Nav goal is {0}".format(g.target_pose.pose))
res = self.nav_client.send_goal_and_wait(\
g, execute_timeout=rospy.Duration(2.0))
loginfo("Nav result was {0}".format(res))
return res
def create_goal(x, y, z, xx, yy, zz, ww):
goal = mb_msgs.MoveBaseGoal()
goal.target_pose.header.frame_id = "map"
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.pose.position.x = x
goal.target_pose.pose.position.y = y
goal.target_pose.pose.position.z = z
goal.target_pose.pose.orientation.x = xx
goal.target_pose.pose.orientation.y = yy
goal.target_pose.pose.orientation.z = zz
goal.target_pose.pose.orientation.w = ww
return goal
def GoTo(self, x, y, theta, frame="map"):
rospy.loginfo("Moving to (%f, %f)" % (x, y))
move_goal = move_base_msgs.MoveBaseGoal()
move_goal.target_pose.pose.position.x = x
move_goal.target_pose.pose.position.y = y
move_goal.target_pose.pose.orientation.z = sin(theta / 2.0)
move_goal.target_pose.pose.orientation.w = cos(theta / 2.0)
move_goal.target_pose.header.frame_id = frame
move_goal.target_pose.header.stamp = rospy.Time(0) #rospy.Time.now()
self.movebase_client_.send_goal(move_goal)
rospy.loginfo(self.movebase_client_.wait_for_result())
rospy.loginfo("Finished moving")
def ros_goal(self, userdata, default_goal):
g = mm.MoveBaseGoal()
p = g.target_pose
p.header.frame_id = 'map'
p.header.stamp = rospy.get_rostime()
p.pose.position.x = self.xy[0]
p.pose.position.y = self.xy[1]
p.pose.position.z = 0
p.pose.orientation.x = self.r[0]
p.pose.orientation.y = self.r[1]
p.pose.orientation.z = self.r[2]
p.pose.orientation.w = self.r[3]
return g
def goto_pose(self, xya, frame_id):
trans, rot = conv.xya_to_trans_rot(xya)
pose = conv.trans_rot_to_pose(trans, rot)
ps = gm.PoseStamped()
ps.pose = pose
ps.header.frame_id = frame_id
ps.header.stamp = rospy.Time(0)
goal = mbm.MoveBaseGoal()
goal.target_pose = ps
goal.header.frame_id = frame_id
rospy.loginfo('Sending move base goal')
finished = self.action_client.send_goal_and_wait(goal)
rospy.loginfo('Move base action returned %d.' % finished)
return finished
def __init__(self, action_client, result_publisher, task_id, pose,
current_task_id):
threading.Thread.__init__(self)
self._current_task_id = task_id
self._result_publisher = result_publisher
self._action_client = action_client
self._response = demo_msgs.ResponseMoveRobot()
self._response.task_id = task_id
self._response.status = False
rospy.loginfo("Sending robot to [%s,%s]" %
(pose.position.x, pose.position.y))
goal = move_base_msgs.MoveBaseGoal()
goal.target_pose.header.frame_id = '/map'
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.pose = pose
self._action_client.send_goal(goal)
def navigateToGoal(self, goal_pose):
# Create the goal point
goal = move_base_msgs.MoveBaseGoal()
goal.target_pose.pose = goal_pose
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.header.frame_id = "map" #odom, map
# Send the goal!
print "sending goal"
self.client.send_goal(goal)
print "waiting for result"
r = rospy.Rate(5)
start_time = rospy.Time.now()
keep_waiting = True
while keep_waiting:
state = self.client.get_state()
#print "State: " + str(state)
if state is not GoalStatus.ACTIVE and state is not GoalStatus.PENDING:
keep_waiting = False
else:
r.sleep()
state = self.client.get_state()
if state == GoalStatus.SUCCEEDED:
return True
return True
def running(self):
rospy.loginfo('[move_base] Moving the arm to a safe configuration...')
move_arm_goal = MoveArmGoal()
move_arm_goal.goal_type = MoveArmGoal.NAMED_TARGET
move_arm_goal.named_target = self.safe_arm_joint_config
self.move_arm_client.send_goal(move_arm_goal)
self.move_arm_client.wait_for_result()
pose = PoseStamped()
if self.goal.goal_type == MoveBaseGoal.NAMED_TARGET:
destination = self.goal.destination_location
rospy.loginfo('[move_base] Moving base to %s', destination)
self.pose = self.convert_pose_name_to_coordinates(destination)
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = self.pose_frame
pose.pose.position.x = self.pose[0]
pose.pose.position.y = self.pose[1]
quat = tf.transformations.quaternion_from_euler(0, 0, self.pose[2])
pose.pose.orientation = Quaternion(*quat)
elif self.goal.goal_type == MoveBaseGoal.POSE:
pose = self.goal.pose
rospy.loginfo('[move_base] Moving base to %s', pose)
else:
rospy.logerr(
'[move_base] Received an unknown goal type; ignoring request')
self.result = self.set_result(False)
return FTSMTransitions.DONE
goal = move_base_msgs.MoveBaseGoal()
goal.target_pose = pose
move_base_client = actionlib.SimpleActionClient(
self.move_base_server, move_base_msgs.MoveBaseAction)
move_base_client.wait_for_server()
move_base_client.send_goal(goal)
success = move_base_client.wait_for_result()
if success:
rospy.loginfo('[move_base] Pose reached successfully')
self.result = self.set_result(True)
return FTSMTransitions.DONE
rospy.logerr('[move_base] Pose could not be reached')
self.result = self.set_result(False)
return FTSMTransitions.DONE
def execute(self, userdata):
pose = PoseStamped()
if userdata.move_base_goal.goal_type == MoveBaseGoal.NAMED_TARGET:
destination = userdata.move_base_goal.destination_location
feedback = MoveBaseFeedback()
feedback.current_state = 'APPROACH_POSE'
feedback.message = '[MOVE_BASE] Moving base to ' + destination
userdata.move_base_feedback = feedback
self.pose = self.convert_pose_name_to_coordinates(destination)
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = self.pose_frame
pose.pose.position.x = self.pose[0]
pose.pose.position.y = self.pose[1]
quat = tf.transformations.quaternion_from_euler(0, 0, self.pose[2])
pose.pose.orientation = Quaternion(*quat)
elif userdata.move_base_goal.goal_type == MoveBaseGoal.POSE:
pose = userdata.move_base_goal.pose
feedback = MoveBaseFeedback()
feedback.current_state = 'APPROACH_POSE'
feedback.message = '[MOVE_BASE] Moving base to {0}'.format(pose)
userdata.move_base_feedback = feedback
else:
rospy.logerr(
'[MOVE_BASE] Received an unknown goal type; ignoring request')
return 'failed'
goal = move_base_msgs.MoveBaseGoal()
goal.target_pose = pose
move_base_client = actionlib.SimpleActionClient(
self.move_base_server, move_base_msgs.MoveBaseAction)
move_base_client.wait_for_server()
move_base_client.send_goal(goal)
success = move_base_client.wait_for_result()
if success:
rospy.loginfo('Pose reached successfully')
return 'succeeded'
rospy.logerr('Pose could not be reached')
return 'failed'
def TurnTo(self, msg, frame="base_link"):
if msg.is_valid:
(theta, self.angle) = TemporalMedian(msg.angle, self.angle)
theta = round((theta / 180.0) * np.pi, 2)
#rospy.loginfo("Turn to (%f)" % theta)
move_goal = move_base_msgs.MoveBaseGoal()
move_goal.target_pose.pose.orientation.z = sin(theta / 2.0)
move_goal.target_pose.pose.orientation.w = cos(theta / 2.0)
move_goal.target_pose.header.frame_id = frame
move_goal.target_pose.header.stamp = rospy.Time(
0) #rospy.Time.now()
if rospy.Time.now() - self.last_trigger_time > rospy.Duration(
3) and self.enabled:
(self.history,
self.history_count) = AddHistory(theta, self.history,
self.history_count)
self.movebase_client_.send_goal(move_goal)
self.last_trigger_time = rospy.Time.now()
def set_pose(self, t, r, frame, block=True):
g = mm.MoveBaseGoal()
p = g.target_pose
p.header.frame_id = frame
p.header.stamp = rospy.get_rostime()
p.pose.position.x = t[0]
p.pose.position.y = t[1]
p.pose.position.z = 0
p.pose.orientation.x = r[0]
p.pose.orientation.y = r[1]
p.pose.orientation.z = r[2]
p.pose.orientation.w = r[3]
self.client.send_goal(g)
if block:
self.client.wait_for_result()
return self.client.get_state()
def execute(self, goal, interrupt_poll_timeout=MOVE_BASE_POLL_TIMEOUT):
"""Executes the (move_base-)action while checking for cancellation.
In case of cancellation, raises an exception. If the goal cannot
be reached, returns False, otherwise True.
"""
self._interrupted = False
goal_msg = move_base_msgs.MoveBaseGoal(target_pose=goal)
self._move_base_action.send_goal(goal_msg)
while not (self._move_base_action.wait_for_result(
rospy.Duration(interrupt_poll_timeout))
or rospy.is_shutdown()):
if self._interrupted:
self._move_base_action.cancel_goal()
self._move_base_action.wait_for_result(
rospy.Duration(PREEMPT_WAIT_FOR_TERMINATION_TIMEOUT))
raise PreemptRequested()
self._move_base_action.get_result()
return self._move_base_action.get_state == actionlib.GoalStatus.SUCCEEDED
def move_to(self, x, y, theta):
"""
Moves base to a 2d pose specified in the map frame. Possible outcomes:
* The function returns successfully. In this case, the robot is guaranteed to be at the goal pose.
* An ActionFailed exception is thrown if navigation fails.
* The robot loops forever. This should only happen in situations involving malicious humans.
"""
goal = mbm.MoveBaseGoal()
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.header.frame_id = '/map'
goal.target_pose.pose = _to_pose(x, y, theta)
rospy.loginfo("Sending base goal ({0}, {1}, {2}) and waiting for result".\
format(x, y, theta))
self._ac.send_goal(goal)
self._ac.wait_for_result()
if self._ac.get_state() != am.GoalStatus.SUCCEEDED:
raise ex.ActionFailedError()
def mapPub():
# 2d Nav Goal
# ask user for a goal if needed, otherwise automatically set by endpoint
#rospy.init_node('mapPub')
rate = rospy.Rate(10)
curfilePath = os.path.abspath(__file__)
curDir = os.path.abspath(os.path.join(curfilePath, os.pardir))
parentDir = os.path.abspath(os.path.join(curDir, os.pardir))
goal = mov_msgs.MoveBaseGoal()
goal.target_pose.header.seq = 0
goal.target_pose.header.stamp = 0
goal.target_pose.header.frame_id = ""
mapInfo = parentDir + "/models/map2.yaml"
with open(mapInfo, 'r') as stream:
try:
yamled = yaml.load(stream)
except yaml.YAMLError as exc:
print(exc)
# deletes info
del yamled['info']
# makes list of location names
# gets locations of each object, attaches them to name
objDict = yamled.values()
objLocations = {}
objNames = {}
objects = {}
for item in objDict:
itemName = item['name']
if itemName[0:4] != "wall":
x_loc = item['centroid_x'] + (item['width'] + .3) * math.cos(math.radians(item['orientation']))
y_loc = item['centroid_y'] + (item['length'] + .3) * math.sin(math.radians(item['orientation']))
quat = tf.transformations.quaternion_from_euler(0, 0, item['orientation']-180)
itemLoc = geo_msgs.Pose(geo_msgs.Point(x_loc, y_loc, 0), geo_msgs.Quaternion(quat[0],quat[1],quat[2],quat[3]))
objLocations[itemName] = itemLoc
objNames[itemName] = ([item['value']])
vertexes = objLocations.values()
vertexKeys = objLocations.keys()
vertexvalues = objNames.values()
status = ['PENDING', 'ACTIVE', 'PREEMPTED',
'SUCCEEDED', 'ABORTED', 'REJECTED',
'PREEMPTING', 'RECALLING', 'RECALLED',
'LOST']
#NAV- STACK
mover_base = actionlib.SimpleActionClient("roy/move_base", mov_msgs.MoveBaseAction)
mover_base.wait_for_server(rospy.Duration(5))
# Keeping track
n_locations = len(vertexes)
for objKey in objLocations.keys():
costs = evaluateFloydCost(self.robLoc, floydWarshallCosts, mapGrid, nextPlace, objLocations[objKey])
newCosts = objNames[objKey] - costs
#floyd-warshall
#costs = np.load('floydWarshallCosts.npy')
#nextPlace = np.load('floydWarshallNextPlace.npy')
#mapGid = np.load('mapGrid.npy')
while not rospy.is_shutdown():
for i in xrange(0, n_locations): #change to iterate through adjusted values
maximum = max(objNames.iteritems(), key=operator.itemgetter(1))[0]
if vertexKeys[i] == maximum:
location = vertexes[i]
rospy.loginfo("Going to " + maximum)
break
else:
location = vertexes[5]
# Set up goal
goal = mov_msgs.MoveBaseGoal()
goal.target_pose.pose = location
goal.target_pose.header.frame_id = 'map'
goal.target_pose.header.stamp = rospy.Time.now()
rospy.loginfo(goal)
# Start the robot toward the next location
mover_base.send_goal(goal)
rospy.loginfo("goal sent")
# Allow 2 minutes to get there
mover_base.wait_for_result(rospy.Duration(120))
# Check status of movement
state = mover_base.get_state()
if state == 3: #SUCCESSFUL
rospy.loginfo(i)
rospy.loginfo("Just Reached " + vertexKeys[i])
del objLocations[vertexKeys[i]]
del objNames[maximum]
else:
rospy.loginfo("Goal failed")
rospy.loginfo(status[state])
mover_base.cancel_goal()
break
rospy.sleep(1)
def running(self):
rospy.loginfo('[move_base] Moving the arm to a safe configuration...')
move_arm_goal = MoveArmGoal()
move_arm_goal.goal_type = MoveArmGoal.NAMED_TARGET
move_arm_goal.named_target = self.safe_arm_joint_config
self.move_arm_client.send_goal(move_arm_goal)
self.move_arm_client.wait_for_result()
pose = PoseStamped()
if self.goal.goal_type == MoveBaseGoal.NAMED_TARGET:
destination = self.goal.destination_location
rospy.loginfo('[move_base] Moving base to %s', destination)
self.pose = self.convert_pose_name_to_coordinates(destination)
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = self.pose_frame
pose.pose.position.x = self.pose[0]
pose.pose.position.y = self.pose[1]
quat = tf.transformations.quaternion_from_euler(0, 0, self.pose[2])
pose.pose.orientation = Quaternion(*quat)
elif self.goal.goal_type == MoveBaseGoal.POSE:
pose = self.goal.pose
rospy.loginfo('[move_base] Moving base to %s', pose)
else:
rospy.logerr(
'[move_base] Received an unknown goal type; ignoring request')
self.result = self.set_result(False)
return FTSMTransitions.DONE
# we attempt a recovery in which we send the robot to a slightly
# different pose from the one originally requested
if self.is_recovering:
pose.pose.position.x += np.random.normal(
0., self.recovery_position_m_std)
pose.pose.position.y += np.random.normal(
0., self.recovery_position_m_std)
goal = move_base_msgs.MoveBaseGoal()
goal.target_pose = pose
self.move_base_goal_pub.publish(pose)
move_base_client = actionlib.SimpleActionClient(
self.move_base_server, move_base_msgs.MoveBaseAction)
move_base_client.wait_for_server()
move_base_client.send_goal(goal)
if move_base_client.wait_for_result(
rospy.Duration.from_sec(self.timeout)):
result = move_base_client.get_result()
resulting_state = move_base_client.get_state()
if result and resulting_state == GoalStatus.SUCCEEDED:
rospy.loginfo('[move_base] Pose reached successfully')
self.reset_state()
self.result = self.set_result(True)
return FTSMTransitions.DONE
else:
rospy.logerr('[move_base] Pose could not be reached')
if self.recovery_count < self.max_recovery_attempts:
self.recovery_count += 1
rospy.logwarn('[move_base] Attempting recovery')
return FTSMTransitions.RECOVER
else:
rospy.logerr(
'[move_base] Pose could not be reached within %f seconds',
self.timeout)
if self.recovery_count < self.max_recovery_attempts:
self.recovery_count += 1
rospy.logerr('[move_base] Attempting recovery')
return FTSMTransitions.RECOVER
rospy.logerr('[move_base] Pose could not be reached; giving up')
self.reset_state()
self.result = self.set_result(False)
return FTSMTransitions.DONE
def create_root(goal=std_msgs.Empty()):
"""
Create the job subtree based on the incoming goal specification.
Args:
goal (:class:`~std_msgs.msg.Empty`): incoming goal specification
Returns:
:class:`~py_trees.behaviour.Behaviour`: subtree root
"""
# beahviors
root = py_trees.composites.Selector(name="Scan or Die")
die = py_trees_ros.tutorials.behaviours.FlashLedStrip(name="Uh Oh",
colour="red")
ere_we_go = py_trees.composites.Sequence(name="Ere we Go")
undock = py_trees_ros.actions.ActionClient(
name="UnDock",
action_namespace="/dock",
action_spec=py_trees_msgs.DockAction,
action_goal=py_trees_msgs.DockGoal(False))
scan_or_be_cancelled = py_trees.composites.Selector(
"Scan or Be Cancelled")
cancelling = py_trees.composites.Sequence("Cancelling?")
is_cancel_requested = py_trees.blackboard.CheckBlackboardVariable(
name="Cancel?",
variable_name='event_cancel_button',
expected_value=True)
move_home_after_cancel = py_trees_ros.actions.ActionClient(
name="Move Home",
action_namespace="/move_base",
action_spec=move_base_msgs.MoveBaseAction,
action_goal=move_base_msgs.MoveBaseGoal())
move_out_and_scan = py_trees.composites.Sequence("Move Out and Scan")
move_base = py_trees_ros.actions.ActionClient(
name="Move Out",
action_namespace="/move_base",
action_spec=move_base_msgs.MoveBaseAction,
action_goal=move_base_msgs.MoveBaseGoal())
scanning = py_trees.composites.Parallel(
name="Scanning",
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
scan_context_switch = py_trees_ros.tutorials.behaviours.ScanContext(
"Context Switch")
scan_rotate = py_trees_ros.actions.ActionClient(
name="Rotate",
action_namespace="/rotate",
action_spec=py_trees_msgs.RotateAction,
action_goal=py_trees_msgs.RotateGoal())
scan_flash_blue = py_trees_ros.tutorials.behaviours.FlashLedStrip(
name="Flash Blue", colour="blue")
move_home_after_scan = py_trees_ros.actions.ActionClient(
name="Move Home",
action_namespace="/move_base",
action_spec=move_base_msgs.MoveBaseAction,
action_goal=move_base_msgs.MoveBaseGoal())
celebrate = py_trees.composites.Parallel(
name="Celebrate",
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
celebrate_flash_green = py_trees_ros.tutorials.behaviours.FlashLedStrip(
name="Flash Green", colour="green")
celebrate_pause = py_trees.timers.Timer("Pause", duration=3.0)
dock = py_trees_ros.actions.ActionClient(
name="Dock",
action_namespace="/dock",
action_spec=py_trees_msgs.DockAction,
action_goal=py_trees_msgs.DockGoal(True))
# Subtree
root.add_children([ere_we_go, die])
ere_we_go.add_children([undock, scan_or_be_cancelled, dock, celebrate])
scan_or_be_cancelled.add_children([cancelling, move_out_and_scan])
cancelling.add_children([is_cancel_requested, move_home_after_cancel])
move_out_and_scan.add_children(
[move_base, scanning, move_home_after_scan])
scanning.add_children(
[scan_context_switch, scan_rotate, scan_flash_blue])
celebrate.add_children([celebrate_flash_green, celebrate_pause])
return root
def __init__(self, copName, robberName):
# Setup node
rospy.init_node('robberEvasion')
# Setup on shutdown
signal.signal(signal.SIGINT, signal_handler)
#rospy.on_shutdown(self.shutDown) # Not Working
# Setup robber service
robberSrv = rospy.Service('robberEvasionGoal', robberEvasionGoal,
self.handleRobberSrv)
self.curRobberGoal = geo_msgs.PoseStamped()
# Retrieve robot locations
rospy.Subscriber("/" + copName + "/base_footprint",
geo_msgs.TransformStamped, self.getCopLocation)
rospy.Subscriber("/" + robberName + "/base_footprint",
geo_msgs.TransformStamped, self.getRobberLocation)
self.copLoc = geo_msgs.PoseStamped()
self.pastCopLoc = geo_msgs.PoseStamped()
self.robLoc = geo_msgs.PoseStamped()
# Setup nav stack
self.mover_base = actionlib.SimpleActionClient(
robberName + "/move_base", mov_msgs.MoveBaseAction)
self.mover_base.wait_for_server(rospy.Duration(5))
status = [
'PENDING', 'ACTIVE', 'PREEMPTED', 'SUCCEEDED', 'ABORTED',
'REJECTED', 'PREEMPTING', 'RECALLING', 'RECALLED', 'LOST'
]
# Get list of objects, locations, and values
curfilePath = os.path.abspath(__file__)
curDir = os.path.abspath(os.path.join(curfilePath, os.pardir))
parentDir = os.path.abspath(os.path.join(curDir, os.pardir))
mapInfo = parentDir + '/models/map2.yaml'
self.objLocations, self.objNames = getObjects(mapInfo)
# Load Floyd Warshall info + map parameters
self.floydWarshallCosts = np.load(parentDir +
'/resources/floydWarshallCosts.npy')
self.floydWarshallNextPlace = np.load(
parentDir + '/resources/floydWarshallNextPlace.npy')
floydYaml = parentDir + '/resources/floydInfo.yaml'
# Get map parameters and mean/std data
meanObjValue, stdObjValue, meanCopCost, stdCopCost = self.getFloydInfo(
floydYaml)
# Map Parameters
# self.originY, self.originX = -3.6, -9.6
# self.mapSizeY, self.mapSizeX = 0.36, 0.68
# Distributions of cost/reward measures
# copSafetyMean = 206.06
# copSafetyStdDev = 148.51
# costMax, meanCost, stdCost = 0, 2.7, 31.4 # self.findMaxCostBased()
self.objValueDistribution = scipy.stats.norm(meanObjValue, stdObjValue)
self.copSafetyDistribution = scipy.stats.norm(
meanCopCost,
stdCopCost) #query with copSafetyDistribution.cdf(value)
# Evasion Parameters
reevaluationTime = 3 # Time to wait before reevaluating the path robber is following
dangerWeight = 1.2 # Amount of danger before robber should choose a new path
self.copDangerVsObjValueWeight = 0.5
# Begin Evasion
while not rospy.is_shutdown():
# Choose destination of least cost
curCost, curDestination = self.floydChooseDestination()
rospy.loginfo("Stealing goods at " + curDestination +
" with danger level of " + str(curCost))
# Travel to destination
goal = mov_msgs.MoveBaseGoal()
goal.target_pose.pose = self.objLocations[curDestination].pose
goal.target_pose.header.frame_id = 'map'
goal.target_pose.header.stamp = rospy.Time.now()
rospy.loginfo(goal)
# Send goal to planner
self.curRobberGoal = self.objLocations[curDestination]
self.mover_base.send_goal(goal)
# Wait for move base to begin
self.mover_base.wait_for_result(rospy.Duration(1))
# While robber is travelling to destination, evaluate the path it is following every few seconds
state = self.mover_base.get_state()
pathFailure = False
while (
pathFailure == False
): #(status[state]=='PENDING' or status[state]=='ACTIVE') and
# Evaluate cost of path
newCost = self.evaluateFloydCost(
self.objLocations[curDestination], curDestination)
print("New Cost: " + str(newCost))
# Check if path is too dangerous
if (newCost > curCost * dangerWeight):
pathFailure = True
# Display Costmap
# copGridLocY, copGridLocX = self.convertPoseToGridLocation(self.copLoc.pose.position.y , self.copLoc.pose.position.x)
# plt.imshow(self.floydWarshallCosts[copGridLocY][copGridLocX]);
# plt.ion()
# plt.show();
# plt.pause(.0001)
# Pause for few seconds until reevaluation of path
rospy.sleep(reevaluationTime)
state = self.mover_base.get_state()
# Check what robber has accomplished
if pathFailure == True: # Robber path is too dangerous, choose a new path
rospy.loginfo(
"Path is too dangerous, finding a new object to steal.")
self.mover_base.cancel_goal()
elif status[state] != 'SUCCEEDED': # Failure in getting to object
rospy.loginfo(
"Robber failed to reach object with error code " +
str(state) + ": " + status[state] +
". Finding something else to steal.")
else: # SUCCESSFUL ROBBERY
rospy.loginfo(
"MWUAHAHAHAHAHA You've successfully stolen valuable goods from the "
+ curDestination)
def create_root():
"""
Create a basic tree and start a 'Topics2BB' work sequence that
takes the asynchronicity out of subscription.
Returns:
:class:`~py_trees.behaviour.Behaviour`: the root of the tree
"""
# Behaviours
root = py_trees.composites.Parallel("Drone Parcel Delivery")
start2bb = py_trees_ros.subscribers.EventToBlackboard(
name="Start2BB",
topic_name="/drone_parcel_delivery/start",
variable_name="event_start_button")
stop2bb = py_trees_ros.subscribers.EventToBlackboard(
name="Stop2BB",
topic_name="/drone_parcel_delivery/stop",
variable_name="event_stop_button")
clear2bb = py_trees_ros.subscribers.EventToBlackboard(
name="Clear2BB",
topic_name="/drone_parcel_delivery/clear",
variable_name="event_clear_button")
topics2bb = py_trees.composites.Sequence("Topics2BB")
add_delivery2bb = dyno_behaviours.add_delivery.ToBlackboard(
name="AddObject2BB", topic_name="/drone_parcel_delivery/add_delivery")
locations2bb = dyno_behaviours.locations.ToBlackboard(
name="Locations2BB", topic_name="/world_state/locations")
objects2bb = dyno_behaviours.objects.ToBlackboard(
name="Objects2BB", topic_name="/world_state/objects")
priorities = py_trees.composites.Selector("Priorities")
idle = py_trees.behaviours.Running(name="Idle")
location_check = py_trees.meta.success_is_failure(
py_trees.composites.Selector)(name="Location problem")
is_queue_empty = py_trees.blackboard.CheckBlackboardVariable(
name="Queue empty?",
variable_name='empty_queue_warning',
expected_value=True)
start = py_trees.meta.success_is_failure(
py_trees.composites.Sequence)(name="Start")
is_start_requested = py_trees.blackboard.CheckBlackboardVariable(
name="Start?", variable_name='event_start_button', expected_value=True)
set_should_move = py_trees.blackboard.SetBlackboardVariable(
name="Set should move",
variable_name='should_move',
variable_value=True)
stop = py_trees.meta.success_is_running(
py_trees.composites.Sequence)(name="Stop")
cancel = py_trees.composites.Sequence(name="Cancel")
is_stop_requested = py_trees.blackboard.CheckBlackboardVariable(
name="Stop?", variable_name='event_stop_button', expected_value=True)
clear_should_move = py_trees.blackboard.SetBlackboardVariable(
name="Clear should move",
variable_name='should_move',
variable_value=False)
move = py_trees.composites.Sequence(name="Move")
move_and_remove = py_trees.composites.Sequence(name="Move")
is_move_requested = py_trees.blackboard.CheckBlackboardVariable(
name="Should move?", variable_name='should_move', expected_value=True)
move_to_next_object = drone_parcel_delivery_behaviours.MoveToNextObject(
name="Move to next object",
action_namespace="/move_base",
action_spec=move_base_msgs.MoveBaseAction,
action_goal=move_base_msgs.MoveBaseGoal())
move_to_next_location = drone_parcel_delivery_behaviours.MoveToNextLocation(
name="Move to next location",
action_namespace="/move_base",
action_spec=move_base_msgs.MoveBaseAction,
action_goal=move_base_msgs.MoveBaseGoal())
pick_up_parcel = drone_parcel_delivery_behaviours.PickUpParcel(
name="Pick up parcel",
action_namespace="/pick_up_parcel",
action_spec=dyno_msgs.QuadrotorPickUpParcelAction,
action_goal=dyno_msgs.QuadrotorPickUpParcelGoal())
drop_off_parcel = drone_parcel_delivery_behaviours.DropOffParcel(
name="Drop off parcel",
action_namespace="/drop_off_parcel",
action_spec=dyno_msgs.QuadrotorPickUpParcelAction,
action_goal=dyno_msgs.QuadrotorPickUpParcelGoal())
move_or_cancel = py_trees.composites.Selector(name="Move or be canceled")
clear = py_trees.meta.success_is_failure(
py_trees.composites.Selector)(name="Clear Queue")
is_clear_requested = py_trees.blackboard.CheckBlackboardVariable(
name="Clear?",
variable_name='event_clear_button',
expected_value=False)
clear_deliveries_in_queue = drone_parcel_delivery_behaviours.ClearDeliveryQueue(
name="Clear deliveries from queue")
remove_first = py_trees.meta.success_is_failure(
py_trees.composites.Sequence)(name="Remove first")
remove_delivery_from_queue = drone_parcel_delivery_behaviours.RemoveFirstDeliveryFromQueue(
name="Remove first delivery from queue")
# Tree
root.add_child(topics2bb)
topics2bb.add_children([
locations2bb, objects2bb, start2bb, stop2bb, clear2bb, add_delivery2bb
])
clear.add_children([is_clear_requested, clear_deliveries_in_queue])
root.add_child(priorities)
priorities.add_children([is_queue_empty, clear, start, move, idle])
start.add_children([is_start_requested, set_should_move])
cancel.add_children([is_stop_requested, clear_should_move])
move.add_children([is_move_requested, move_or_cancel])
move_or_cancel.add_children([cancel, move_and_remove])
move_and_remove.add_children([
move_to_next_object, pick_up_parcel, move_to_next_location,
drop_off_parcel, remove_delivery_from_queue
])
return root
def create_root():
"""
Create a basic tree and start a 'Topics2BB' work sequence that
takes the asynchronicity out of subscription.
Returns:
:class:`~py_trees.behaviour.Behaviour`: the root of the tree
"""
# Behaviours
root = py_trees.composites.Parallel("Route Scheduler")
start2bb = py_trees_ros.subscribers.EventToBlackboard(
name="Start2BB",
topic_name="/route_scheduler/start",
variable_name="event_start_button")
stop2bb = py_trees_ros.subscribers.EventToBlackboard(
name="Stop2BB",
topic_name="/route_scheduler/stop",
variable_name="event_stop_button")
clear2bb = py_trees_ros.subscribers.EventToBlackboard(
name="Clear2BB",
topic_name="/route_scheduler/clear",
variable_name="event_clear_button")
topics2bb = py_trees.composites.Sequence("Topics2BB")
add_location2bb = dyno_behaviours.add_location.ToBlackboard(
name="AddLocation2BB", topic_name="/route_scheduler/add_location")
locations2bb = dyno_behaviours.locations.ToBlackboard(
name="Locations2BB", topic_name="/world_state/locations")
priorities = py_trees.composites.Selector("Priorities")
idle = py_trees.behaviours.Running(name="Idle")
location_check = py_trees.meta.success_is_failure(
py_trees.composites.Selector)(name="Location problem")
is_queue_empty = py_trees.blackboard.CheckBlackboardVariable(
name="Queue empty?",
variable_name='empty_queue_warning',
expected_value=True)
start = py_trees.meta.success_is_failure(
py_trees.composites.Sequence)(name="Start")
is_start_requested = py_trees.blackboard.CheckBlackboardVariable(
name="Start?", variable_name='event_start_button', expected_value=True)
set_should_move = py_trees.blackboard.SetBlackboardVariable(
name="Set should move",
variable_name='should_move',
variable_value=True)
stop = py_trees.meta.success_is_running(
py_trees.composites.Sequence)(name="Stop")
cancel = py_trees.composites.Sequence(name="Cancel")
is_stop_requested = py_trees.blackboard.CheckBlackboardVariable(
name="Stop?", variable_name='event_stop_button', expected_value=True)
clear_should_move = py_trees.blackboard.SetBlackboardVariable(
name="Clear should move",
variable_name='should_move',
variable_value=False)
move = py_trees.composites.Sequence(name="Move")
move_and_remove = py_trees.composites.Sequence(name="Move")
is_move_requested = py_trees.blackboard.CheckBlackboardVariable(
name="Should move?", variable_name='should_move', expected_value=True)
move_to_next = route_scheduler_behaviours.MoveToNextWaypoint(
name="Move to next waypoint",
action_namespace="/move_base",
action_spec=move_base_msgs.MoveBaseAction,
action_goal=move_base_msgs.MoveBaseGoal())
move_or_cancel = py_trees.composites.Selector(name="Move or be canceled")
clear = py_trees.meta.success_is_failure(
py_trees.composites.Selector)(name="Clear Queue")
is_clear_requested = py_trees.blackboard.CheckBlackboardVariable(
name="Clear?",
variable_name='event_clear_button',
expected_value=False)
clear_locations_in_queue = route_scheduler_behaviours.ClearLocationQueue(
name="Clear locations from queue")
remove_first = py_trees.meta.success_is_failure(
py_trees.composites.Sequence)(name="Remove first")
remove_location_from_queue = route_scheduler_behaviours.RemoveFirstLocationFromQueue(
name="Remove first location from queue")
# Tree
root.add_child(topics2bb)
topics2bb.add_children(
[locations2bb, start2bb, stop2bb, clear2bb, add_location2bb])
clear.add_children([is_clear_requested, clear_locations_in_queue])
root.add_child(priorities)
priorities.add_children([is_queue_empty, clear, start, move, idle])
start.add_children([is_start_requested, set_should_move])
cancel.add_children([is_stop_requested, clear_should_move])
move.add_children([is_move_requested, move_or_cancel])
move_or_cancel.add_children([cancel, move_and_remove])
move_and_remove.add_children([move_to_next, remove_location_from_queue])
return root
def create_root():
# behaviours
root = py_trees.composites.Parallel("Tutorial")
topics2bb = py_trees.composites.Sequence("Topics2BB")
scan2bb = py_trees_ros.subscribers.EventToBlackboard(
name="Scan2BB",
topic_name="/dashboard/scan",
variable_name="event_scan_button"
)
cancel2bb = py_trees_ros.subscribers.EventToBlackboard(
name="Cancel2BB",
topic_name="/dashboard/cancel",
variable_name="event_cancel_button"
)
battery2bb = py_trees_ros.battery.ToBlackboard(name="Battery2BB",
topic_name="/battery/state",
threshold=30.0
)
priorities = py_trees.composites.Selector("Priorities")
battery_check = py_trees.meta.success_is_failure(py_trees.composites.Selector)(name="Battery Emergency")
is_battery_ok = py_trees.blackboard.CheckBlackboardVariable(
name="Battery Ok?",
variable_name='battery_low_warning',
expected_value=False
)
flash_led_strip = py_trees_ros.tutorials.behaviours.FlashLedStrip(
name="Flash Red",
colour="red")
scan = py_trees.composites.Sequence(name="Scan")
is_scan_requested = py_trees.blackboard.CheckBlackboardVariable(
name="Scan?",
variable_name='event_scan_button',
expected_value=True
)
scan_or_die = py_trees.composites.Selector(name="Scan or Die")
die = py_trees_ros.tutorials.behaviours.FlashLedStrip(
name="Uh Oh",
colour="red")
ere_we_go = py_trees.composites.Sequence(name="Ere we Go")
undock = py_trees_ros.actions.ActionClient(
name="UnDock",
action_namespace="/dock",
action_spec=py_trees_msgs.DockAction,
action_goal=py_trees_msgs.DockGoal(False),
override_feedback_message_on_running="undocking"
)
scan_or_be_cancelled = py_trees.composites.Selector("Scan or Be Cancelled")
cancelling = py_trees.composites.Sequence("Cancelling?")
is_cancel_requested = py_trees.blackboard.CheckBlackboardVariable(
name="Cancel?",
variable_name='event_cancel_button',
expected_value=True
)
move_home_after_cancel = py_trees_ros.actions.ActionClient(
name="Move Home",
action_namespace="/move_base",
action_spec=move_base_msgs.MoveBaseAction,
action_goal=move_base_msgs.MoveBaseGoal()
)
move_out_and_scan = py_trees.composites.Sequence("Move Out and Scan")
move_base = py_trees_ros.actions.ActionClient(
name="Move Out",
action_namespace="/move_base",
action_spec=move_base_msgs.MoveBaseAction,
action_goal=move_base_msgs.MoveBaseGoal()
)
scanning = py_trees.composites.Parallel(name="Scanning", policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
scan_context_switch = py_trees_ros.tutorials.behaviours.ScanContext("Context Switch")
scan_rotate = py_trees_ros.actions.ActionClient(
name="Rotate",
action_namespace="/rotate",
action_spec=py_trees_msgs.RotateAction,
action_goal=py_trees_msgs.RotateGoal(),
override_feedback_message_on_running="rotating"
)
scan_flash_blue = py_trees_ros.tutorials.behaviours.FlashLedStrip(name="Flash Blue", colour="blue")
move_home_after_scan = py_trees_ros.actions.ActionClient(
name="Move Home",
action_namespace="/move_base",
action_spec=move_base_msgs.MoveBaseAction,
action_goal=move_base_msgs.MoveBaseGoal()
)
celebrate = py_trees.composites.Parallel(name="Celebrate", policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
celebrate_flash_green = py_trees_ros.tutorials.behaviours.FlashLedStrip(name="Flash Green", colour="green")
celebrate_pause = py_trees.timers.Timer("Pause", duration=3.0)
dock = py_trees_ros.actions.ActionClient(
name="Dock",
action_namespace="/dock",
action_spec=py_trees_msgs.DockAction,
action_goal=py_trees_msgs.DockGoal(True),
override_feedback_message_on_running="docking"
)
idle = py_trees.behaviours.Running(name="Idle")
# tree
root.add_children([topics2bb, priorities])
topics2bb.add_children([scan2bb, cancel2bb, battery2bb])
priorities.add_children([battery_check, scan, idle])
battery_check.add_children([is_battery_ok, flash_led_strip])
scan.add_children([is_scan_requested, scan_or_die])
scan_or_die.add_children([ere_we_go, die])
ere_we_go.add_children([undock, scan_or_be_cancelled, dock, celebrate])
scan_or_be_cancelled.add_children([cancelling, move_out_and_scan])
cancelling.add_children([is_cancel_requested, move_home_after_cancel])
move_out_and_scan.add_children([move_base, scanning, move_home_after_scan])
scanning.add_children([scan_context_switch, scan_rotate, scan_flash_blue])
celebrate.add_children([celebrate_flash_green, celebrate_pause])
return root
def execute(self, userdata):
frame_id = userdata['frame_id']
x = userdata['x']
y = userdata['y']
theta = userdata['theta']
try:
collision_aware = userdata['collision_aware']
except KeyError:
rospy.loginfo("setting missing input collision_aware to True")
collision_aware = True
#switch global and local planners and costmap params for move_base to be collision-aware or not
reconfig_str = "rosrun dynamic_reconfigure dynparam set " + self.move_base_node_name
if collision_aware:
rospy.loginfo(
"navigate_to_pose: switching to collision-aware nav planners")
move_base_config = ''' "{'base_global_planner': 'navfn/NavfnROS', 'base_local_planner': 'dwa_local_planner/DWAPlannerROS'}" '''
costmap_config = '''/local_costmap "{'max_obstacle_height': 2.0, 'inflation_radius': 0.55 }" '''
error = subprocess.call(reconfig_str + move_base_config,
shell=True)
if error:
rospy.logerr(
"navigate_to_pose: dynamic reconfigure for move_base_node failed!"
)
return 'error'
error = subprocess.call(reconfig_str + costmap_config, shell=True)
if error:
rospy.logerr(
"navigate_to_pose: dynamic reconfigure for costmap_config failed!"
)
return 'error'
goal = mbm.MoveBaseGoal()
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.header.frame_id = frame_id
goal.target_pose.pose = _to_pose(x, y, theta)
rospy.loginfo(
"Sending base goal (%f, %f, %f) and waiting for result" %
(x, y, theta))
self.move_base_client.send_goal(goal)
r = rospy.Rate(10)
while not rospy.is_shutdown():
if self.preempt_requested():
self.move_base_client.cancel_goal()
self.service_preempt()
return 'preempted'
state = self.move_base_client.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("navigation succeeded")
return 'succeeded'
elif state not in [GoalStatus.PENDING, GoalStatus.ACTIVE]:
rospy.loginfo("state was:" + str(state))
return 'failed'
r.sleep()
#non-collision-aware version just sends commands to the base controller for awhile
else:
#only use for small, base-relative commands!
if frame_id not in [
'base_link', 'base_footprint', '/base_link',
'/base_footprint'
]:
rospy.logerr(
"non-collision-aware commands should be in base_link or base_footprint!"
)
return 'failed'
if abs(x) > 0.5:
rospy.logerr("x was too large, clipping to 0.5 m")
x = 0.5 * x / abs(x)
if abs(y) > 0.5:
rospy.logerr("y was too large, clipping to 0.5 m")
y = 0.5 * y / abs(y)
#send constant-speed commands until (in theory) we could have traveled that far
time = max([abs(x) / .2, abs(y) / .2, abs(theta) / .25])
r = rospy.Rate(10)
steps = int(math.floor(time * 10))
for i in range(steps):
if self.preempt_requested():
return 'preempted'
base_cmd = gm.Twist()
base_cmd.linear.x = 10. * x / steps
base_cmd.linear.y = 10. * y / steps
base_cmd.angular.z = 10. * theta / steps
self.cmd_vel_pub.publish(base_cmd)
r.sleep()
return 'succeeded'
def simple_goal_cb(msg):
mbf_cmb_ac.send_goal(mb_msgs.MoveBaseGoal(target_pose=msg))
def __init__(self, copName, robberName):
# Setup node
rospy.init_node('copEvader')
# rospy.on_shutdown(self.shutDown)
# Retrieve robot locations
rospy.Subscriber("/" + copName + "/base_footprint",
geo_msgs.TransformStamped, self.getCopLocation)
rospy.Subscriber("/" + robberName + "/base_footprint",
geo_msgs.TransformStamped, self.getRobberLocation)
self.copLoc = geo_msgs.PoseStamped()
self.pastCopLoc = geo_msgs.PoseStamped()
self.robLoc = geo_msgs.PoseStamped()
# Setup nav stack
self.mover_base = actionlib.SimpleActionClient(
robberName + "/move_base", mov_msgs.MoveBaseAction)
self.mover_base.wait_for_server(rospy.Duration(5))
status = [
'PENDING', 'ACTIVE', 'PREEMPTED', 'SUCCEEDED', 'ABORTED',
'REJECTED', 'PREEMPTING', 'RECALLING', 'RECALLED', 'LOST'
]
# Get list of objects and their locations
curfilePath = os.path.abspath(__file__)
curDir = os.path.abspath(os.path.join(curfilePath, os.pardir))
parentDir = os.path.abspath(os.path.join(curDir, os.pardir))
mapInfo = parentDir + '/models/map2.yaml'
self.objLocations = getObjects(mapInfo)
vertexes = self.objLocations.values()
vertexKeys = self.objLocations.keys()
# Load Floyd Warshall info
self.mapGrid = np.load(parentDir + '/resources/mapGrid.npy')
self.floydWarshallCosts = np.load(
parentDir + '/resources/floydWarshallCosts20.npy')
self.floydWarshallNextPlace = np.load(
parentDir + '/resources/floydWarshallNextPlace.npy')
# Evasion Parameters
reevaluationTime = 3 # Time to wait before reevaluating the path robber is following
dangerWeight = 0.75 # Amount of danger before robber should choose a new path
# Map Parameters
self.originY, self.originX = -3.6, -9.6 # ????
# mapSizeY, mapSizeX = 0.18, 0.34
self.mapSizeY, self.mapSizeX = 0.36, 0.68 # ?? is this map size in meters???
# print(self.findMaxCost())
costDistribution = scipy.stats.norm(206.06, 148.51)
print(costDistribution.cdf(206.06))
# Begin Evasion
while not rospy.is_shutdown():
# Choose destination of least cost
curCost, curDestination = self.floydChooseDestination()
rospy.loginfo("Stealing goods at " + curDestination +
" with danger level of " + str(curCost))
# Travel to destination
goal = mov_msgs.MoveBaseGoal()
goal.target_pose.pose = self.objLocations[curDestination].pose
goal.target_pose.header.frame_id = 'map'
goal.target_pose.header.stamp = rospy.Time.now()
rospy.loginfo(goal)
self.mover_base.send_goal(goal)
# Would this make it so you wait for 2 seconds every time?
# mover_base.wait_for_result(rospy.Duration(2))
# While robber is travelling to destination, evaluate the path it is following every few seconds
state = self.mover_base.get_state()
pathFailure = False
while (status[state] == 'PENDING'
or status[state] == 'ACTIVE') and (pathFailure == False):
# Evaluate cost of path
newCost = self.evaluateFloydCost(
self.objLocations[curDestination])
print("New Cost: " + str(newCost))
# Check if path is too dangerous
if newCost < curCost * dangerWeight:
pathFailure = True
# Display Costmap
copGridLocY, copGridLocX = self.convertPoseToGridLocation(
self.copLoc.pose.position.y, self.copLoc.pose.position.x)
plt.imshow(self.floydWarshallCosts[copGridLocY][copGridLocX])
plt.ion()
plt.show()
plt.pause(.0001)
# Pause for few seconds until reevalutation of path
rospy.sleep(reevaluationTime)
state = self.mover_base.get_state()
# Check what robber has accomplished
if pathFailure == True: # Robber path is too dangerous, choose a new path
rospy.loginfo(
"Path is too dangerous, finding a new object to steal.")
self.mover_base.cancel_goal()
elif status[state] != 'SUCCEEDED': # Failure in getting to object
rospy.loginfo(
"Robber failed to reach object with error code " +
str(state) + ": " + status[state] +
". Finding something else to steal.")
else: # SUCCESSFUL ROBBERY
rospy.loginfo(
"MWUAHAHAHAHAHA You've successfully stolen valuable goods from the "
+ curDestination)
