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)
