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)