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
