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
