def send_move_base_goal(posx, posy, rotationz): global last_goal_id ag = MoveBaseActionGoal() ag.header = Header() ag.goal_id = GoalID() ID = "0" if(not last_goal_id == None): print "Last goal id:" print last_goal_id IDint = int(float(last_goal_id.id))+1 ID = str(IDint) print "ID:" print ID print "\n" #ag.header.seq = ID ag.goal_id.id = ID print ag.goal_id ag.goal = MoveBaseGoal() ag.goal.target_pose.header.frame_id = "map" ag.goal.target_pose.pose.position.x = posx ag.goal.target_pose.pose.position.y = posy dummy = tf.transformations.quaternion_from_euler(0, 0, rotationz) ag.goal.target_pose.pose.orientation.x = dummy[0] ag.goal.target_pose.pose.orientation.y = dummy[1] ag.goal.target_pose.pose.orientation.z = dummy[2] ag.goal.target_pose.pose.orientation.w = dummy[3] print ag pub_goal.publish(ag) last_goal_id = ag.goal_id pub_log.publish(rospy.get_name()+": published ActionGoal")
def get_goal_to_publish(): # Get a random goal and go there # We could try to improve this random search, and have some memory global latest_goal_time global latest_goal_id global goal_points_list if latest_goal_id == None: latest_goal_id = 1 latest_goal_time = time.time() else: if time.time() - latest_goal_time > 20.: latest_goal_id += 1 latest_goal_time = time.time() else: # Print should not happen anymore print "Too little time passed since last goal sent" return None goal_to_send = MoveBaseActionGoal() current_goal_id = GoalID() current_goal_id.id = str(latest_goal_id) goal_to_send.goal_id = current_goal_id pose_stamped = PoseStamped() pose = Pose() idx = random.randrange(len(goal_points_list)) pose.position.x = goal_points_list[idx][0] pose.position.y = goal_points_list[idx][1] roll = 0. pitch = 0. yaw = 0. quaternion = tf.transformations.quaternion_from_euler(roll, pitch, yaw) pose.orientation.x = quaternion[0] pose.orientation.y = quaternion[1] pose.orientation.z = quaternion[2] pose.orientation.w = quaternion[3] pose_stamped.pose = pose goal = MoveBaseGoal() goal.target_pose = pose_stamped goal.target_pose.header.frame_id = 'map' goal_to_send.goal = goal return goal_to_send
def get_action(self): mba = MoveBaseAction() ag_ = MoveBaseActionGoal() ag_.header = Header() ag_.header.frame_id = "map" ag_.header.stamp = rospy.Time.now() ag_.goal = MoveBaseGoal() ag_.goal.target_pose = self.target_waypoint mba.action_goal = ag_ return mba
def generate_goal(): global goal pose_stamped = PoseStamped() pose_stamped.pose = None # no goal at start move_base_goal = MoveBaseGoal() move_base_goal.target_pose = pose_stamped goal = MoveBaseActionGoal() goal.goal = move_base_goal
def compute_initial_pose(self): self.initial_pose = rospy.wait_for_message('/pose', PoseStamped) rospy.loginfo("The default position is: {}".format(self.initial_pose)) # Trasfrom from PoseStamped to MoveBaseActionGoal. move_base = MoveBaseGoal(self.initial_pose) move_base_action = MoveBaseActionGoal() move_base_action.goal = move_base move_base_action.goal.target_pose.header.frame_id = "map" move_base_action.header.stamp.secs = self.initial_pose.header.stamp.secs move_base_action.header.stamp.nsecs = self.initial_pose.header.stamp.nsecs move_base_action.header.frame_id = "map" self.goal = move_base_action
def goTo(name): # TODO: Implement this. global navPending global needsCancel global callback navPending = True if name == 'lower torso': torso = fetch_api.Torso() torso.set_height(0.0) navPending = False return 0 elif name == 'raise torso': torso = fetch_api.Torso() torso.set_height(0.4) navPending = False return 0 else: if not os.path.exists('pickle/' + name): print 'No such pose \'' + name + '\'' navPending = False return 1 else: loadfile = open('pickle/' + name, 'rb') stampedCoPose = pickle.load(loadfile) loadfile.close() # TODO: Figure out why this goal doesn't cause any motion mbagoal = MoveBaseActionGoal() mbgoal = MoveBaseGoal() mbgoal.target_pose.header.frame_id = 'map' mbgoal.target_pose.header.stamp = rospy.Time().now() mbgoal.target_pose.pose = stampedCoPose.pose #potential issue here mbagoal.goal = mbgoal #mbagoal.header = #mbagoal.goal_id.id = 'ios' pub.publish(mbagoal) #pub.send_goal(mbgoal) # END TODO rospy.sleep(5) while callback.motion: print 'waiting' rospy.sleep(1) navPending = False callback.motion = True return 0 navPending = False return 1
def driveTo(x,y, theta): rospy.loginfo("Drive to (%f, %f, %f)", x, y, theta) newPose = generate_pose(x, y, 0) #print newPose actionGoal = MoveBaseActionGoal() actionGoal.header = genHeader() actionGoal.goal_id.id = str(driveTo.goalID) actionGoal.goal_id.stamp = rospy.Time.now() goal = MoveBaseGoal() goal.target_pose = newPose actionGoal.goal = goal # Publish the goal to the robot global actionGoalPublisher actionGoalPublisher.publish(actionGoal) global soundClient # Wait for the robot's status to to have reached the goal timeOutCounter = 0 while not rospy.is_shutdown(): # This is done so that status can be checked and used rospy.sleep(4.) timeOutCounter += 1 currentStatus = getStatus(driveTo.goalID) global cant_reach_list print "Status: %d, GoalID: %d, Driving to: (%f, %f, %f), # unreachable: %d" % (currentStatus, driveTo.goalID, x, y, theta, len(cant_reach_list )) if currentStatus == GoalStatus.ABORTED or timeOutCounter > 20: soundClient.say("Abort driving to goal.") print "The goal was aborted" cant_reach_list.append((x, y)) break elif currentStatus == GoalStatus.REJECTED: soundClient.say("Goal rejected") print "The goal was rejected" break elif currentStatus == GoalStatus.LOST: soundClient.say("Robot is lost") print "The robot is lost, exiting driving" #TODO Should we send a cancel message? exit(1) break elif currentStatus == GoalStatus.SUCCEEDED: soundClient.say("Goal reached. Moving on.") print "Drive to complete!" break driveTo.goalID += 1
def move_to_goal(self, goal): mbag = MoveBaseActionGoal() # First Header mbag.header.frame_id = '' # Use the map frame to define goal poses mbag.header.seq = self.goal_id mbag.header.stamp = rospy.Time.now() # Now the Goal ID mbag.goal_id.stamp = rospy.Time.now() mbag.goal_id.id = "g_" + str(self.goal_id) # Now the goal, which we already have as an argument mbag.goal = goal #PoseStamped(Pose(Point(2.0, -3.0, 0.0), quaternion_from_euler(0, 0, -1.5706, axes='sxyz'))) self.goal_setter.publish(mbag)
def get_goals(self, file): # Read in the next goal pose. Set it up, return to send goal method. #FORMAT: #x, y; run_identfier = time.time() goal_list = [] clockwise = True if run_identfier % 2 == 0 else False run_identfier = str( run_identfier) + "_clock" if clockwise else "_counterclock" with open(file, 'r') as goals_file: goals_reader = csv.reader(goals_file, delimiter=',') # for i in range(5): for row_num, row in enumerate(goals_reader): MBAG = MoveBaseActionGoal() MBAG.goal_id = GoalID() MBAG.goal_id.id = str(run_identfier) + "_path_" + str(row_num) MBAG.header = Header() MBAG.header.frame_id = 'map' MBG = MoveBaseGoal() MBG.target_pose.header.frame_id = "map" MBG.target_pose.pose.position.x = float(row[0]) MBG.target_pose.pose.position.y = float(row[1]) MBG.target_pose.pose.orientation.w = 1.0 MBAG.goal = MBG goal_list.append(MBAG) #Switch directions half the time if clockwise: temp = goal_list[0] goal_list[0] = goal_list[2] goal_list[2] = temp rospy.loginfo("Pathing clockwise: " + str(clockwise)) return goal_list