def publishNextPoint(self): x_next, y_next = self.convert_node_to_point(self.next_node) w_next = self.computeNextOrientation(x_next, y_next) rospy.loginfo( "[%s] Converted next node to point: (%s,%s), orientation: %s" % (self.node_name, x_next, y_next, w_next)) goal_id = GoalID() goal_id.id = str(self.goal_idx) msg = MoveBaseActionGoal() msg.goal_id = goal_id msg.goal.target_pose.header.frame_id = self.frame_id msg.goal.target_pose.pose.position.x = x_next msg.goal.target_pose.pose.position.y = y_next msg.goal.target_pose.pose.position.z = 0 msg.goal.target_pose.pose.orientation.x = 0 msg.goal.target_pose.pose.orientation.y = 0 msg.goal.target_pose.pose.orientation.z = math.sin(w_next / 2) msg.goal.target_pose.pose.orientation.w = math.cos(w_next / 2) self.pub_goal.publish(msg) rospy.loginfo("[%s] Dispatched goal point: (%s,%s)" % (self.node_name, x_next, y_next)) if self.next_node == self.goal: self.dispatched_end = True
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 add_waypoint(self, goal_with_priority, add_after_current = False, need_tf = False): # GoalWithPriority goal_id = self.new_goal_id() pose = goal_with_priority.pose goal_with_priority.goal_id = goal_id move_base_msg = MoveBaseActionGoal() move_base_msg.header.frame_id = 'odom' move_base_msg.goal.target_pose.header.frame_id = 'odom' move_base_msg.goal.target_pose.pose = pose move_base_msg.goal_id = goal_id goal = GoalManager.GoalWrapper(move_base_msg, goal_with_priority, need_tf = need_tf) goal.goal_with_priority = goal_with_priority if add_after_current: self.goals.insert(self.current_idx + 1, goal) else: self.goals.append(goal) rospy.loginfo("Received goal:\n\tx:%f\n\ty:%f" % (pose.position.x, pose.position.y))
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 add_waypoint(self, goal_with_priority, add_after_current=False, need_tf=False): # GoalWithPriority goal_id = self.new_goal_id() pose = goal_with_priority.pose goal_with_priority.goal_id = goal_id move_base_msg = MoveBaseActionGoal() move_base_msg.header.frame_id = 'odom' move_base_msg.goal.target_pose.header.frame_id = 'odom' move_base_msg.goal.target_pose.pose = pose move_base_msg.goal_id = goal_id goal = GoalManager.GoalWrapper(move_base_msg, goal_with_priority, need_tf=need_tf) goal.goal_with_priority = goal_with_priority if add_after_current: self.goals.insert(self.current_idx + 1, goal) else: self.goals.append(goal) rospy.loginfo("Received goal:\n\tx:%f\n\ty:%f" % (pose.position.x, pose.position.y))
def publishNextPoint(self): x_next, y_next = self.convert_node_to_point(self.next_node) w_next = self.computeNextOrientation(x_next,y_next) rospy.loginfo("[%s] Converted next node to point: (%s,%s), orientation: %s" %(self.node_name, x_next, y_next, w_next)) goal_id = GoalID() goal_id.id = str(self.goal_idx) msg = MoveBaseActionGoal() msg.goal_id = goal_id msg.goal.target_pose.header.frame_id = self.frame_id msg.goal.target_pose.pose.position.x = x_next msg.goal.target_pose.pose.position.y = y_next msg.goal.target_pose.pose.position.z = 0 msg.goal.target_pose.pose.orientation.x = 0 msg.goal.target_pose.pose.orientation.y = 0 msg.goal.target_pose.pose.orientation.z = math.sin(w_next/2) msg.goal.target_pose.pose.orientation.w = math.cos(w_next/2) self.pub_goal.publish(msg) rospy.loginfo("[%s] Dispatched goal point: (%s,%s)" %(self.node_name,x_next,y_next)) if self.next_node == self.goal: self.dispatched_end = True