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 go_to_location(self, location, frame_id, timeout=50.0): ''' description: move the robot to the desired location input: string location, the name of the location that you want the robot to go double timeout, max time to allow to reach the location before returning string frame_id, frame where the pose is located NOTE: the location needs to be loaded in the parameter server in advance, to get a list of availbale locations use mbot.get_available_locations() function output: bool success, whether if the robot was succesful or not at reaching the goal ''' timeout_ros = rospy.Duration(timeout) time_start = rospy.Time.now() try: location_pose = self.get_available_poses()[location] except KeyError: rospy.logerr( '{} is not a valid waypoint from this list: {}'.format( location, self.get_available_poses())) return None goal = PoseStamped() if not isinstance(frame_id, str): frame_id = 'odom' goal.header.frame_id = frame_id goal.header.stamp = rospy.Time.now() quaternion = tf.transformations.quaternion_from_euler( 0, 0, location_pose[2]) goal.pose.orientation.x = quaternion[0] goal.pose.orientation.y = quaternion[1] goal.pose.orientation.z = quaternion[2] goal.pose.orientation.w = quaternion[3] goal.pose.position.x = location_pose[0] goal.pose.position.y = location_pose[1] goal.pose.position.z = 0 final_goal = MoveBaseActionGoal() final_goal.header = goal.header final_goal.goal_id.stamp = rospy.Time.now() final_goal.goal_id.id = str(random()) goal_id = final_goal.goal_id final_goal.goal.target_pose = goal self.__move_base_safe_pub.publish(final_goal) rospy.loginfo("Sending goal to move_base_simple, destination : " + location) while not rospy.is_shutdown( ) and rospy.Time.now() - time_start < timeout_ros: if self.__goals_status: for i in self.__goals_status: if i.goal_id == goal_id and i.status == 3: return True return False
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 go_to_pose(self, location_pose, frame_id, back_up=[1, 1], timeout=50): ''' description: move the robot to the desired pose needs: mir_move_base_safe and mbot_actions move_base_server input: PoseStamped location_pose, x, y, yaw double timeout, max time to allow to reach the location before returning string frame_id, frame where the pose is located output: bool success, whether if the robot was succesful or not at reaching the goal ''' timeout_ros = rospy.Duration(timeout) time_start = rospy.Time.now() goal = PoseStamped() if not isinstance(frame_id, str): frame_id = 'odom' goal.header.frame_id = frame_id goal.header.stamp = rospy.Time.now() quaternion = tf.transformations.quaternion_from_euler( 0, 0, location_pose[2]) goal.pose.orientation.x = quaternion[0] goal.pose.orientation.y = quaternion[1] goal.pose.orientation.z = quaternion[2] goal.pose.orientation.w = quaternion[3] goal.pose.position.x = location_pose[0] goal.pose.position.y = location_pose[1] goal.pose.position.z = 0 final_goal = MoveBaseActionGoal() final_goal.header = goal.header final_goal.goal_id.stamp = rospy.Time.now() final_goal.goal_id.id = str(random()) goal_id = final_goal.goal_id final_goal.goal.target_pose = goal self.__move_base_safe_pub.publish(final_goal) rospy.loginfo("Sending goal to move_base_simple, destination : (" + str(location_pose[0]) + "," + str(location_pose[1]) + ") with yaw: " + str(location_pose[2])) while not rospy.is_shutdown( ) and rospy.Time.now() - time_start < timeout_ros: if self.__goals_status: for i in self.__goals_status: if i.goal_id == goal_id and i.status == 3: return True if i.goal_id == goal_id and i.status == 4: return self.go_to_pose([ location_pose[0] + back_up[0], location_pose[1] + back_up[1], location_pose[2] ], frame_id=frame_id, back_up=back_up, timeout=timeout) return False
def goalcallback(self, data): #we'll send a goal to the robot to move 3 meters forward goal = MoveBaseGoal() # goal.target_pose.header.frame_id = 'base_link' # goal.target_pose.header.stamp = rospy.Time.now() # goal.target_pose.pose.position.x = 0.2 # goal.target_pose.pose.orientation.w = 1.0 goal.target_pose = data #start moving rospy.logerr(goal) # self.move_base.send_goal(goal) #TODO check status of route, if on another route, cancel it first # self.cancelMovement() t = rospy.Time.now() #draws the path req = GetPlanRequest() req.start = self.robot_pose_stamped req.start.header.stamp = t req.goal = data req.goal.header.stamp = t req.tolerance = .02 # return_path = self.get_path_to_goal_srv(req) # self.next_path_pub.publish(return_path) self.move_to_goal_count += 1 g = MoveBaseActionGoal(); t = rospy.Time.now() g.header = Header(self.move_to_goal_count, t, "map") g.goal_id.stamp = t g.goal_id.id = "movement_num:"+str(self.move_to_goal_count) g.goal.target_pose = data # newnppose = vector3_to_numpy(data.pose.position) # if np.linalg.norm(newnppose - self.lastnppose) >.05 : # self.goal_pose_stamped_pub.publish(data) self.move_to_goal_pub.publish(g) rospy.logerr("published move_base goal")
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 goalcallback(self, data): #we'll send a goal to the robot to move 3 meters forward goal = MoveBaseGoal() # goal.target_pose.header.frame_id = 'base_link' # goal.target_pose.header.stamp = rospy.Time.now() # goal.target_pose.pose.position.x = 0.2 # goal.target_pose.pose.orientation.w = 1.0 goal.target_pose = data #start moving rospy.logerr(goal) # self.move_base.send_goal(goal) #TODO check status of route, if on another route, cancel it first # self.cancelMovement() t = rospy.Time.now() #draws the path req = GetPlanRequest() req.start = self.robot_pose_stamped req.start.header.stamp = t req.goal = data req.goal.header.stamp = t req.tolerance = .02 # return_path = self.get_path_to_goal_srv(req) # self.next_path_pub.publish(return_path) self.move_to_goal_count += 1 g = MoveBaseActionGoal() t = rospy.Time.now() g.header = Header(self.move_to_goal_count, t, "map") g.goal_id.stamp = t g.goal_id.id = "movement_num:" + str(self.move_to_goal_count) g.goal.target_pose = data # newnppose = vector3_to_numpy(data.pose.position) # if np.linalg.norm(newnppose - self.lastnppose) >.05 : # self.goal_pose_stamped_pub.publish(data) self.move_to_goal_pub.publish(g) rospy.logerr("published move_base goal")
def talker(self,data): numPath = len(data.poses) for i in range(numPath): pub_msg = MoveBaseActionGoal() pub_msg.goal.target_pose = data.poses[i] pub_msg.header = data.header pub_msg.goal.target_pose.header.frame_id = 'map' pub_msg.header.frame_id = 'map' # pub_msg.goal.target_pose.header.frame_id = data.poses[i].header.frame_id pub_msg.goal.target_pose.pose.position.x = data.poses[i].pose.position.x-2 pub_msg.goal.target_pose.pose.position.y = data.poses[i].pose.position.y-2 pub_msg.goal.target_pose.pose.position.z = data.poses[i].pose.position.z # pub_msg.goal.target_pose.pose.orientation = data.poses[i].pose.orientation if(self.RecFlag): self.pub.publish(pub_msg) rospy.sleep(2) print(pub_msg) # flag=True self.RecFlag=False
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
def send_fridge_goal(self): """ Send move_base a pose goal to go to the fridge using action server """ print "SENDING GOAL" goal_action = MoveBaseActionGoal() goal = PoseStamped() header = std_msgs.msg.Header() header.stamp = rospy.Time.now() pose = Pose() goal.header = header goal.pose = Pose(position=Point(x=100.136801147, y=100.06081869507, z=0.0), orientation=Quaternion(x=0.0, y=0.0, z=-0.523665174293, w=0.851924166363)) header.frame_id = 'map' goal_action.header = header goal_action.goal.target_pose = goal self.base_goal_pub.publish(goal_action)
def sendToOnce(): global PUB_GOAL, PUBD # if PUBD: # return # PUBD = True DISTANCE_THRESHOLD = .1 print'********' # x = data.pose.pose.position.x # y = data.pose.pose.position.y # if math.sqrt(x**2+y**2) < DISTANCE_THRESHOLD: # return ps = PoseStamped() ps.header.frame_id = "/map" ps.header.stamp = rospy.Time.now() ps.pose.position.x = 1 ps.pose.position.y = 0 ps.pose.position.z = 0 q = quaternion_from_euler(0, 0, 0, 'sxyz') ps.pose.orientation.x = q[0] ps.pose.orientation.y = q[1] ps.pose.orientation.z = q[2] ps.pose.orientation.w = q[3] mbag = MoveBaseActionGoal() mbag.header = ps.header mbag.goal.target_pose = ps print mbag PUB_GOAL.publish(mbag)
def publish_goal(): global centroids global finished global goal_counter global failed global full_fail global last_goal global last_none_flag global goal global init init= PoseStamped() init.pose.position.x=0 init.pose.position.y=0 init.pose.position.z=0 init.pose.orientation.x=0 init.pose.orientation.y=0 init.pose.orientation.z=1 goal_counter += 1 # print "dajkhfjajlfjdlka" if len(centroids) > 0: print "Publishing goal %s" % goal_counter # get the first centroid first = centroids.pop(0) # check if it is too close dist = math.sqrt((first.pose.position.x - newPose.pose.position.x)**2 + (first.pose.position.y - newPose.pose.position.y)**2) # if last_goal != None: # last_none_flag = 0 # last_dist = 999 # if not last_none_flag: # last_dist = math.sqrt((last_goal.pose.position.x - newPose.pose.position.x)**2 + (last_goal.pose.position.y - newPose.pose.position.y)**2) # create a goal goal = MoveBaseActionGoal() goal.header = first.header goal.goal_id.id = "Goal%s" % goal_counter goal.goal.target_pose = first goal_pub.publish(goal) last_goal = first # else: # print "Goal too close" # # goal_counter -= 1 # if len(centroids) > 0: # publish_goal() # elif full_fail > 3: # finished = 1 # else: # full_fail += 1 # run_navigation(False) else: # there are no reachable centroids if failed: finished = 1 goal = MoveBaseActionGoal() goal.goal_id.id = "Goal%s" % goal_counter goal.goal.target_pose = init goal_pub.publish(goal) else: failed = 1 goal = MoveBaseActionGoal() goal.goal_id.id = "Goal%s" % goal_counter goal.goal.target_pose = init goal_pub.publish(goal) run_navigation(False)