def go_home(self):
self._client.cancel_all_goals()
print "Going home"
self.drawNodes([self._starting_node] * 2, "path")
self._current_node = self._starting_node
new_goal_pose = PoseStamped()
new_goal_pose.header.seq = 1
new_goal_pose.header.frame_id = 'map'
new_goal_pose.header.stamp = rospy.Time.now()
p = self.mapCoordsToWorld(self._starting_node.x, self._starting_node.y)
new_goal_pose.pose.position.x = p[0]
new_goal_pose.pose.position.y = p[1]
new_goal_pose.pose.position.z = 0.0
q = quaternion_from_euler(0.0, 0.0, 0.0)
new_goal_pose.pose.orientation = Quaternion(*q)
goal = MoveBaseGoal()
goal.target_pose = new_goal_pose
self._client.send_goal(goal)
def go_back_home_cb(userdata, default_goal):
rospy.logdebug("Entered go back home callback.")
go_back_goal = MoveBaseGoal()
go_back_goal.target_pose = userdata.gb_cb_waypoints[2] # waypoint 3
go_back_goal.target_pose.header.frame_id = 'map'
go_back_goal.target_pose.header.stamp = rospy.Time.now()
return go_back_goal
def TestCallback(msg):
goal_x = 1
goal_y = 0
goal_yaw = 0
print "Goal Subscribe"
print "Move to (" + str(goal_x) + " , " + str(goal_y) + " , " + str(goal_yaw) + ")\n"
cli = actionlib.SimpleActionClient('/move_base', MoveBaseAction)
cli.wait_for_server()
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = "base_link"
pose.pose.position = Point(goal_x, goal_y, 0)
quat = tf.transformations.quaternion_from_euler(0, 0, goal_yaw)
pose.pose.orientation = Quaternion(*quat)
goal = MoveBaseGoal()
goal.target_pose = pose
cli.send_goal(goal)
cli.wait_for_result()
action_state = cli.get_state()
if action_state == GoalStatus.SUCCEEDED:
rospy.loginfo("Navigation Succeeded.")
def waypoint_pub(self, lat, lon):
utm_conversion = utm.fromLatLong(lat, lon)
print(utm_conversion)
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = "utm"
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.pose.position.x = utm_conversion.easting
goal.target_pose.pose.position.y = utm_conversion.northing
goal.target_pose.pose.position.z = 0
goal.target_pose.pose.orientation.w = 1.0
try:
transform = self.tf_buffer.lookup_transform(
"map",
goal.target_pose.header.frame_id, #source frame
rospy.Time(0), #get the tf at first available time
rospy.Duration(2.0)) #wait for 1 second
goal.target_pose = tf2_geometry_msgs.do_transform_pose(
goal.target_pose, transform)
#goal.target_pose.pose.position.x = -goal.target_pose.pose.position.x
goal.target_pose.header.stamp = rospy.Time.now()
print "goal transformed\n", goal
self.mb_client.send_goal(goal)
wait = self.mb_client.wait_for_result()
return wait
except:
print "Couldnt find transform from utm to map"
return False
def execute(self, ud):
self.move_base_client.wait_for_server()
goal = MoveBaseGoal()
goal.target_pose = ud.move_waypoints[ud.move_target_wp]
goal.target_pose.header.stamp = rospy.Time.now()
self.move_base_client.send_goal(goal)
self.move_base_client.wait_for_result()
status = self.move_base_client.get_state()
tts_msg = String()
if status == GoalStatus.SUCCEEDED:
tts_msg.data = "I have reached " + ud.move_wp_str[
ud.move_target_wp] + "."
self.tts_pub.publish(tts_msg)
if ud.move_target_wp == 0:
return 'test_finished'
else:
ud.move_target_wp -= 1
return 'succeeded'
else:
return 'wp_not_reached'
def sendGoalDoneCallback(terminalState, result):
global waypointsPatrolList, currentWaypointIndex, patrolId
rospy.loginfo("Received waypoint terminal state : [%s]",
getStatusString(terminalState))
# Check if goal was successfully achieved
if terminalState != Status.SUCCEEDED.value:
# Stop patrol if goal could not be reached
rospy.loginfo("Goal was not reached: terminating patrol")
publishPatrolFeedBack(patrolId, "failed", currentWaypointIndex,
len(waypointsPatrolList))
return
else:
currentWaypointIndex += 1
# Check if all waypoints are done
if currentWaypointIndex >= len(waypointsPatrolList):
publishPatrolFeedBack(patrolId, "finished", currentWaypointIndex,
len(waypointsPatrolList))
rospy.loginfo("Patrol done. All waypoints reached.")
# Check if the patrol has to be restarted
if isLooped == True:
rospy.loginfo("Restarting patrol with same waypoints...")
startPatrolNavigation()
# Proceed to next waypoint of the patrol
else:
publishPatrolFeedBack(patrolId,
"waypoint_reached", currentWaypointIndex,
len(waypointsPatrolList))
goal = MoveBaseGoal()
goal.target_pose = waypointsPatrolList[currentWaypointIndex][
REAL_POSESTAMPED_INDEX]
rospy.loginfo("Processing next waypoint...")
actionClient.send_goal(goal, sendGoalDoneCallback)
def navigate(self, goalHandle, statusCB, doneCB):
positions = goalHandle.get_goal().jointPositions
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = "map"
pose.pose.position.x = positions[0]
pose.pose.position.y = positions[1]
pose.pose.position.z = 0.0
q = Rotation.RotZ(positions[2]).GetQuaternion()
pose.pose.orientation.x = q[0]
pose.pose.orientation.y = q[1]
pose.pose.orientation.z = q[2]
pose.pose.orientation.w = q[3]
client_goal = MoveBaseGoal()
client_goal.target_pose = pose
client = self._jointControllers.get('base', None)
if client:
client.wait_for_server()
rospy.loginfo("%s: Navigating to %s",
rospy.get_name(),
pose)
client.send_goal(client_goal, done_cb=doneCB)
else:
goalHandle.set_rejected()
def send_goal(self, startPose, targetPose, action):
self.fid_subscriber.unregister()
goal = MoveBaseGoal()
goal.target_pose = startPose
self.client.send_goal(goal)
print("Start goal published\n", self.closest_fiducial.fiducial_id)
success = self.client.wait_for_result()
if success and self.client.get_state() == GoalStatus.SUCCEEDED:
print("Goal reached")
if action == "GO":
print("Action: GO")
self.sendTargetGoal(targetPose)
print("Target goal published!")
elif action == "STOP":
print("Action: STOP")
input("Press any key to continue")
self.sendTargetGoal(targetPose)
else:
self.client.cancel_goal()
return self.client.get_result()
def MoveCallback(self, msg):
goal_x = msg.pose.position.x
goal_y = msg.pose.position.y
print "Goal Subscribe\n"
print "Move to (" + str(goal_x) + " , " + str(goal_y) + ")"
cli = actionlib.SimpleActionClient('/move_base/move', MoveBaseAction)
cli.wait_for_server()
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = "map"
pose.pose.position = Point(goal_x, goal_y, 0)
pose.pose.orientation.x = 0
pose.pose.orientation.y = 0
pose.pose.orientation.z = msg.pose.orientation.z
pose.pose.orientation.w = msg.pose.orientation.w
goal = MoveBaseGoal()
goal.target_pose = pose
cli.send_goal(goal)
cli.wait_for_result()
action_state = cli.get_state()
if action_state == GoalStatus.SUCCEEDED:
rospy.loginfo("Navigation Succeeded.")
self.goal_signal.publish(True)
else:
rospy.loginfo("Navigation Failed.")
self.goal_signal.publish(False)
def waypoint_action_client(self):
waypoint_client = actionlib.SimpleActionClient('move_base',
MoveBaseAction)
print "Waiting"
waypoint_client.wait_for_server()
goal = MoveBaseGoal()
no_of_wp = self.path_manager.global_path.poses.__len__()
print no_of_wp
while self.path_manager.global_path.poses.__len__() != 0:
print self.path_manager.global_path.poses.__len__()
#if no_of_wp == self.path_manager.global_path.poses.__len__():
# goal.is_first_wp = True
#else:
# goal.is_first_wp = False
goal.target_pose = self.path_manager.global_path.poses.pop(0)
#if self.path_manager.global_path.poses.__len__() == 0:
# goal.is_last_wp = True
print goal
waypoint_client.send_goal(goal)
print "goal sent"
waypoint_client.wait_for_result() #rospy.Duration.from_sec(10.0)
print "ferri"
def choose_new_goal(self):
# Don't choose a new goal if map is complete
if self._mapComplete:
return
# Wait for a closest node to be set
while self._closest_node is None:
rospy.sleep(0.5)
self._client.cancel_all_goals()
print "Choosing new goal"
self.drawNodes([self._closest_node] * 2, "path")
self._current_node = self._closest_node
new_goal_pose = PoseStamped()
new_goal_pose.header.seq = 1
new_goal_pose.header.frame_id = 'map'
new_goal_pose.header.stamp = rospy.Time.now()
p = self.mapCoordsToWorld(self._closest_node.x, self._closest_node.y)
new_goal_pose.pose.position.x = p[0]
new_goal_pose.pose.position.y = p[1]
new_goal_pose.pose.position.z = 0.0
q = quaternion_from_euler(0.0, 0.0, 0.0)
new_goal_pose.pose.orientation = Quaternion(*q)
goal = MoveBaseGoal()
goal.target_pose = new_goal_pose
self._client.send_goal(goal, self.goal_completed)
def execute(self, ud):
self.move_base_client.wait_for_server()
goal = MoveBaseGoal()
pose = PoseStamped()
pose.header.frame_id = 'map'
pose.header.stamp = rospy.Time.now()
pose.pose.position.x = 2.0
pose.pose.position.y = 0.0
pose.pose.orientation.x = 0.0
pose.pose.orientation.y = 0.0
pose.pose.orientation.z = 0.0
pose.pose.orientation.w = 1.0
goal.target_pose = pose
goal.target_pose.header.stamp = rospy.Time.now()
self.move_base_client.send_goal(goal)
self.move_base_client.wait_for_result(rospy.Duration(30))
status = self.move_base_client.get_state()
if status == GoalStatus.SUCCEEDED:
return 'succeeded'
else:
return 'supervise'
def move_other_frame(self, target_pose, frame='camera_link', retry=True):
if self.enabled:
target_pose = self.cam_pose_to_base_pose(target_pose, frame)
target_pose = transform_pose('map', target_pose)
target_pose.pose.position.z = 0
self.goal_pub.publish(target_pose)
goal = MoveBaseGoal()
goal.target_pose = target_pose
if self.knowrob is not None:
self.knowrob.start_base_movement(self.knowrob.pose_to_prolog(target_pose))
while True:
self.client.send_goal(goal)
wait_result = self.client.wait_for_result(rospy.Duration(self.timeout))
result = self.client.get_result()
state = self.client.get_state()
if wait_result and state == GoalStatus.SUCCEEDED:
break
if retry:
cmd = raw_input('base movement did not finish in time, retry? [y/n]')
retry = cmd == 'y'
if not retry:
print('movement did not finish in time')
if self.knowrob is not None:
self.knowrob.finish_action()
raise TimeoutError()
if self.knowrob is not None:
self.knowrob.finish_action()
return result
def MoveCallback(self, msg):
goal_x = msg.pose.position.x
goal_y = msg.pose.position.y
#rad = math.atan2(goal_y,goal_x)
rad = random.uniform(-(math.pi),(math.pi))
print "Goal Subscribe\n"
print "Move to (" + str(goal_x) + " , " + str(goal_y) + " , " + str(rad*180/math.pi) + ")"
cli = actionlib.SimpleActionClient('/move_base/move', MoveBaseAction)
cli.wait_for_server()
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = "map"
pose.pose.position = Point(goal_x, goal_y, 0)
quat = tf.transformations.quaternion_from_euler(0, 0, rad)
pose.pose.orientation = Quaternion(*quat)
goal = MoveBaseGoal()
goal.target_pose = pose
cli.send_goal(goal)
cli.wait_for_result()
action_state = cli.get_state()
if action_state == GoalStatus.SUCCEEDED:
rospy.loginfo("Navigation Succeeded.")
self.goal_signal.publish(True)
def _move_to_marker(self, trans_pose):
pose = trans_pose
goal = MoveBaseGoal()
goal.target_pose = pose
# The robot can't move in these directions. Clear them so it's not confused
goal.target_pose.pose.position.z = 0
goal.target_pose.pose.orientation.x = 0
goal.target_pose.pose.orientation.y = 0
# If the marker position is fresh, stay behind it so we don't collide.
# Use trans_pose to get original time
if self._last_trans_pose.header.stamp + rospy.Duration(2) > rospy.Time.now():
goal.target_pose.pose.position.x = goal.target_pose.pose.position.x - self.FOLLOW_DIST
rospy.logdebug(
"Sending new goal. x: "
+ str(goal.target_pose.pose.position.x)
+ ", y: "
+ str(goal.target_pose.pose.position.y)
)
# Cancel old goal if it exists.
self.base_client.cancel_all_goals()
self.base_client.send_goal(goal)
def move_base_goal_cb(userdata, default_goal):
rospy.logdebug("Entered move_base goal callback.")
move_goal = MoveBaseGoal()
move_goal.target_pose = userdata.mb_cb_waypoints[userdata.mb_cb_target_wp - 1]
move_goal.target_pose.header.stamp = rospy.Time.now()
# print move_goal.target_pose
return move_goal
def _send_action_goal(self, x, y, theta, frame):
"""A function to send the goal state to the move_base action server """
goal = MoveBaseGoal()
goal.target_pose = build_pose_msg(x, y, theta, frame)
goal.target_pose.header.stamp = rospy.Time.now()
rospy.loginfo("Waiting for the server")
self.move_base_sac.wait_for_server()
rospy.loginfo("Sending the goal")
self.move_base_sac.send_goal(goal)
rospy.sleep(0.1)
rospy.loginfo("Waiting for the Result")
while True:
assert (
self.execution_status is not 4), \
"move_base failed to find a valid plan to goal"
if self.execution_status is 3:
rospy.loginfo("Base reached the goal state")
return
if self.base_state.should_stop:
rospy.loginfo(
"Base asked to stop. Cancelling goal sent to move_base.")
self.cancel_goal()
return
def execute(self, ud):
self.move_base_client.wait_for_server()
goal = MoveBaseGoal()
goal.target_pose = ud.move_waypoints[ud.move_target_wp]
goal.target_pose.header.stamp = rospy.Time.now()
self.move_base_client.send_goal(goal)
self.move_base_client.wait_for_result()
status = self.move_base_client.get_state()
tts_msg = String()
if status == GoalStatus.SUCCEEDED:
tts_msg.data = "I have reached " + ud.move_wp_str[ud.move_target_wp] + "."
self.tts_pub.publish(tts_msg)
if ud.move_target_wp == 0:
return 'test_finished'
else:
ud.move_target_wp -= 1
return 'succeeded'
else:
return 'wp_not_reached'
def navigate_to(self, nav_goal_pose):
rospy.loginfo(
"navigating to %f %f" %
(nav_goal_pose.pose.position.x, nav_goal_pose.pose.position.y))
goal = MoveBaseGoal()
goal.target_pose = nav_goal_pose
self.move_base_client.send_goal_and_wait(goal, rospy.Duration(60 * 5),
rospy.Duration(5))
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 update(self):
global cancel_goto_action, moving
if not self.done and cancel_goto_action:
cancel_goto_action = False
self.goto_ac.cancel_all_goals()
self.tried = False
clear_costmaps_req = self.clear_costmaps_srv()
return pt.common.Status.RUNNING
if self.start_over_count < start_over_handler:
self.done = False
self.tried = False
self.start_over_count += 1
if not self.done and not self.tried:
if self.tag == "table1":
goal = MoveBaseGoal()
goal.target_pose = self.table1_msg
goal.target_pose.header.stamp = rospy.Time()
self.goto_ac.send_goal(goal)
elif self.tag == "table2":
goal = MoveBaseGoal()
goal.target_pose = self.table2_msg
goal.target_pose.header.stamp = rospy.Time()
self.goto_ac.send_goal(goal)
self.tried = True
print('###################################################')
return pt.common.Status.RUNNING
# if succesful
if self.done:
moving = False
return pt.common.Status.SUCCESS
else:
moving = True
state = self.goto_ac.get_state()
if state == GoalStatus.SUCCEEDED:
self.done = True
if state == GoalStatus.ABORTED:
self.tried = False
return pt.common.Status.FAILURE
def simple_goal_cb(self, goal_pose):
"""
Callback for the 2D nav goal button in rviz.
:param goal_pose: goal position for the nav_p controller
:type goal_pose: PoseStamped
"""
g = MoveBaseGoal()
g.target_pose = goal_pose
self.self_client.send_goal(g)
def sendTargetGoal(self, targetPose):
self.fid_subscriber = rospy.Subscriber("/fiducial_transforms",
FiducialTransformArray,
self.fiducial_callback)
self.closest_fiducial = None
goal = MoveBaseGoal()
goal.target_pose = targetPose
self.client.send_goal(goal)
def move_action(destination_x, destination_y, poseX, poseY):
global POS_TOLERANCE
global STOP
goodTermination = False
rate = rospy.Rate(2)
# initialize action client
cli = actionlib.SimpleActionClient('/move_base/move', MoveBaseAction)
# wait for the action server to establish connection
cli.wait_for_server()
goal_begin = PoseStamped()
initial_x = poseX
initial_y = poseY
initialDistance = math.sqrt((destination_x - initial_x)**2 +
(destination_y - initial_y)**2)
destination_angle = math.atan2(destination_y - initial_y,
destination_x - initial_x)
#the action takes in a posestamped
print "destinationx, y and angle: ", destination_x, destination_y, destination_angle
goal_begin.header.stamp = rospy.Time.now()
goal_begin.header.frame_id = "map"
goal_begin.pose.position = Point(destination_x, destination_y, 0)
quat = quaternion_from_euler(0, 0, destination_angle)
goal_begin.pose.orientation = Quaternion(*quat)
goal = MoveBaseGoal()
goal.target_pose = goal_begin
# send message to the action server
cli.send_goal(goal)
# wait for the action server to complete the order
cli.wait_for_result(rospy.Duration(180)) #3min
#the result send a number corresponding to success or failure or what happened. 4: path avorted (something similar) 3:ok 0:ok?
#check_result(cli)
#if delay passed we stop the robot here and there with the goal topic.
if STOP == True:
odom = getodom()
poseX, poseY, posew = odom.get()
print("actual pose", poseX, poseY)
response_move = move_action(poseX, poseY, poseX, poseY)
STOP = False
#rospy.sleep(2)
# print result of navigation
action_state = cli.get_state()
if action_state == GoalStatus.SUCCEEDED:
goodTermination = True
print("goal reached")
return goodTermination
def move_base_simple_callback(self, msg):
goal = MoveBaseGoal()
goal.target_pose = msg
ac_move_base = actionlib.SimpleActionClient(self.move_base_action_name, MoveBaseAction)
rospy.loginfo("Waiting for ActionServer: %s", self.move_base_action_name)
if not ac_move_base.wait_for_server(rospy.Duration(1)):
rospy.logerr("Emulator move_base simple failed because move_base action server is not available")
return
rospy.loginfo("send goal to %s", self.move_base_action_name)
ac_move_base.send_goal(goal)
def move_to(self, stamped, done_cb, timeout_cb): goal = MoveBaseGoal() goal.target_pose = stamped goal.target_pose.header.stamp = rospy.Time(0) msg = MarkerArray() msg.markers.append(self.tools.create_pose_marker(stamped)) self.tools.visualization_publisher.publish(msg) self.cancel_all_actions() self.cancel_standstill_timer() self._action = ActionWrapper(done_cb=done_cb, timeout_cb=timeout_cb, timeout=Params().move_base_timeout, action_client=self.move_base_client, goal=goal)
def go_to(self, goal_posestamped, timeout=0):
goal = MoveBaseGoal()
goal.target_pose = goal_posestamped
self.movebase.send_goal(goal)
self.movebase.wait_for_result() # timeout maybe?
# result = self.movebase.get_goal_status_text()
# print 'move_base ' + result
resultcode = self.movebase.get_state()
# print 'result code ' + repr(resultcode)
return resultcode
def go_to(self, goal_posestamped, timeout=0):
goal = MoveBaseGoal()
goal.target_pose = goal_posestamped
self.movebase.send_goal(goal)
self.movebase.wait_for_result() # timeout maybe?
# result = self.movebase.get_goal_status_text()
# print 'move_base ' + result
resultcode = self.movebase.get_state()
# print 'result code ' + repr(resultcode)
return resultcode
def go_to(self, pose_stamped):
"""Goes to a location in the world.
Args:
pose_stamped: geometry_msgs/PoseStamped. The location to go to.
"""
self._move_base_client.wait_for_server()
goal = MoveBaseGoal()
goal.target_pose = pose_stamped
self._move_base_client.send_goal(goal)
self._move_base_client.wait_for_result()
def run(argv=None):
rospy.init_node(NAME, anonymous=False)
print "Waiting for services to come up"
rospy.wait_for_service('wide_get_checkerboard_pose')
rospy.wait_for_service('narrow_get_checkerboard_pose')
rospy.wait_for_service('get_approach_pose')
try:
print "Building a tf history"
pose_pub = rospy.Publisher("nav_pose", PoseStamped)
#print "Tucking the arms"
##tuck the arms
#tuck_arm_client = actionlib.SimpleActionClient('tuck_arms', TuckArmsAction)
#tuck_arm_client.wait_for_server()
#tuck_goal = TuckArmsGoal(True,True)
#tuck_arm_client.send_goal_and_wait(tuck_goal)
print "Attempting to get the pose of the board"
#Get the pose of the 3x4 checkerboard
get_checkerboard_pose = rospy.ServiceProxy('wide_get_checkerboard_pose', GetCheckerboardPose)
board_pose = get_checkerboard_pose(5, 4, .08, .08).board_pose
print "Attempting to get approach pose"
#given the pose of the checkerboard, get a good pose to approach it from
get_approach_pose = rospy.ServiceProxy('get_approach_pose', GetApproachPose)
nav_pose = get_approach_pose(board_pose).nav_pose
#publish the final pose for debugging purposes
pose_pub.publish(nav_pose)
print "Waiting for move_base_local"
#OK... our nav_pose is now ready to be sent to the navigation stack as a goal
move_base_client = actionlib.SimpleActionClient('move_base_local', MoveBaseAction)
move_base_client.wait_for_server()
goal = MoveBaseGoal()
goal.target_pose = nav_pose
print "Attempting to approach table"
#send the goal and wait for the base to get there
move_base_client.send_goal_and_wait(goal)
#Get the pose of the 5x4 checkerboard on the box
#get_checkerboard_pose = rospy.ServiceProxy('wide_get_checkerboard_pose', GetCheckerboardPose)
#board_pose = get_checkerboard_pose(5, 4, 0.022, 0.022).board_pose
#Grab the box
#grab_box = rospy.ServiceProxy('grab_box', GrabBox)
#grab_box(board_pose)
except rospy.ServiceException, e:
rospy.logerr("The service call to get the checkerboard pose failed: %s" % e)
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 collision_goal_cb(userdata, goal):
tf_listener = tf.TransformListener()
pose = PoseStamped()
pose.header.frame_id = '/base_link'
pose.pose.position.x = 2.0 # Default 2.0
pose.pose.orientation.w = 1
goal = MoveBaseGoal()
goal.target_pose = pose
return goal
def point_2_base_goal(point, frame_id="map", orientation=None):
goal = PoseStamped()
goal.header.frame_id = frame_id
if orientation is not None:
goal.pose.orientation = orientation
else:
goal.pose.orientation.w = 1
goal.pose.position = point
goal.header.stamp = rospy.Time(0)
move_base_goal = MoveBaseGoal()
move_base_goal.target_pose = goal
return move_base_goal
def move_to_pos(X, Y, Rotation):
pose = PoseStamped()
pose.header.frame_id = "base_link"
pose.pose.orientation.x = 0
pose.pose.orientation.y = 0
pose.pose.orientation.z = math.radians(Rotation)
pose.pose.orientation.w = 1
pose.pose.position.x = X
pose.pose.position.y = Y
goal = MoveBaseGoal()
goal.target_pose = pose
client.send_goal(goal)
def tick(self):
if self.status is "idle":
self.status = "running"
goal_pose = MoveBaseGoal()
goal_pose.target_pose = self.pose
goal_pose.target_pose.header.frame_id = 'map'
self.move_client.send_goal(goal_pose)
return "running"
elif self.status is "running":
if self.move_client.get_result():
return "success"
return "running"
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 retreat(self, done_cb, timeout_cb):
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = 'base_footprint'
goal.target_pose.pose.position.x = -(Params().approach_distance - Params().ring_distance)
goal.target_pose.header.stamp = rospy.Time(0)
goal.target_pose.pose.orientation.w = 1
goal.target_pose = self.tools.transform_pose('map', goal.target_pose)
msg = MarkerArray()
msg.markers.append(self.tools.create_pose_marker(goal.target_pose))
msg.markers[-1].color.r = 0.8
msg.markers[-1].color.g = 0.5
self.tools.visualization_publisher.publish(msg)
self.cancel_all_actions()
self.cancel_standstill_timer()
self._action = ActionWrapper(done_cb=done_cb, timeout_cb=timeout_cb, timeout=Params().approach_timeout, action_client=self.retreat_client, goal=goal)
def execute(self, userdata):
rospy.loginfo("MoveBase called.")
try:
client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
client.wait_for_server(rospy.Duration.from_sec(10.0))
goal = MoveBaseGoal()
goal.target_pose = userdata.goal_pose
client.send_goal(goal)
client.wait_for_result(rospy.Duration.from_sec(80.0))
if client.get_state() == 3:
return 'succeeded'
else:
return 'failed'
except actionlib.ActionException, e:
rospy.logwarn("MoveBase action failed: %s" %e)
return 'failed'
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_pose(self, pos, frame="/odom_combined"):
"""
Move the robot to pos, using the move_base actions.
@param pos: A PoseStamped
@param frame: Check that the frame is allowed by move_base
"""
if not self.use_move_base:
rospy.logerr("Trying to call a move_base service without \
initializing it")
self.listener.waitForTransform(frame, pos.header.frame_id,
rospy.Time(0), rospy.Duration((1.0)))
target = self.listener.transformPose(frame, pos)
goal= MoveBaseGoal()
goal.target_pose = target
self.move_action.send_goal(goal)
return self.move_action.wait_for_result()
def move(self, goal_x=0, goal_y=0, goal_yaw=0, x_lower_bound=-1, x_upper_bound=1, y_lower_bound=-1, y_upper_bound=1):
# To prevent robot getting stuck or drifting towards just one specific area over course of experiment
# Randomly send to middle of allowable area with 1% probability
rand = random.uniform(0, 100)
if rand == 1:
goal_x = (x_lower_bound + x_upper_bound) / 2
goal_y = (y_lower_bound + y_upper_bound) / 2
goal_pose = PoseStamped()
goal_pose.header.stamp = rospy.Time.now()
goal_pose.header.frame_id = 'map'
goal_pose.pose.position.x = goal_x
goal_pose.pose.position.y = goal_y
goal_pose.pose.orientation = self.continuous_pose_wrt_map.pose.pose.orientation # Always keep our orientation the same
goal = MoveBaseGoal()
goal.target_pose = goal_pose
try:
rospy.logdebug('Waiting for move_base server')
self.move_base_client.wait_for_server()
rospy.logdebug('Sending goal to move_base server')
self.move_base_client.send_goal(goal)
rospy.logdebug('Waiting for goal result from move_base server')
self.move_base_client.wait_for_result()
success = self.move_base_client.get_result()
if not success:
rospy.logwarn(self.namespace + ': move_base was not able to successfully complete the request action')
else:
continuous_x = self.continuous_pose_wrt_map.pose.pose.position.x
continuous_y = self.continuous_pose_wrt_map.pose.pose.position.y
discrete_x = self.discrete_pose_wrt_map.pose.pose.position.x
discrete_y = self.discrete_pose_wrt_map.pose.pose.position.y
gazebo_x = self.gazebo_pose_wrt_map.pose.pose.position.x
gazebo_y = self.gazebo_pose_wrt_map.pose.pose.position.y
rospy.loginfo(self.namespace + ': requested move to (' + str(goal_x) + ', ' + str(goal_y) + ') ' +
'completed. Current gazebo odom pose is (' + str(gazebo_x) + ', ' + str(gazebo_y) + '). ' +
'Current continuous odom pose is (' + str(continuous_x) + ', ' + str(continuous_y) + '). ' +
'Current discrete odom pose is (' + str(discrete_x) + ', ' + str(discrete_y) + '). ')
except rospy.ROSException as e:
rospy.logwarn(self.namespace + ': caught exception while sending move_base a movement goal - ' + e.message)
def set_goal(self, goal_id):
if self.current_goal_status() != "FREE":
self.cancel_goal()
# invalid goal id
if goal_id >= len(self.waypoints):
self.goal_status = "ERROR"
rospy.logerr("Invalid goal id: " + str(goal_id))
return
self.current_goal_id = goal_id
self.goal_status = "WORKING"
goal = MoveBaseGoal()
goal.target_pose = self.waypoints[goal_id]
def done_cb(status, result):
self.goal_status = "FREE"
# wait for 1s
if not self.move_base.wait_for_server(rospy.Duration(1)):
self.goal_status = "ERROR"
self.cancel_goal()
return
self.move_base.send_goal(goal, done_cb=done_cb)
if len(goal_list) == 0:
all_bad_cnt += 1
if all_bad_cnt > 2:
sounds.publish(Sound(0))
rospy.sleep(2.)
sounds.publish(Sound(1))
rospy.sleep(2.)
sys.exit()
break
goal = goal_list.pop(0)[1]
test_plan = do_astar(goal, pose, 0.0).plan.poses
print test_plan
goal_lock = False
dist = pose_dist(pose, goal)
print dist
#path = do_astar(pose, goal, 0.0).plan
go_pub.publish(goal)
pub_goal = MoveBaseGoal()
pub_goal.target_pose = goal
go_action.send_goal(pub_goal)
result = go_action.wait_for_result()
goal_blacklist.append(goal)
goal_list.pop(0)
if go_action.get_state() == 4:
do_circle = False
else:
do_circle = True
except Exception as exc:
print exc
def person_position_goal_cb(userdata, nav_goal):
move_base_goal = MoveBaseGoal()
move_base_goal.target_pose = userdata.person_pose
move_base_goal.target_pose.header.stamp = rospy.Time.now()
return move_base_goal
def begin_explore( self, radius, preempt_func = retfalse ):
rospy.logout( 'snaking_explore: setting radius' )
waypoints = self.setup_poses( radius ) # PoseStamped in self.frame
local_planner = rospy.get_param('/move_base_node/base_local_planner') # : dwa_local_planner/DWAPlannerROS
local_planner = local_planner[string.find(local_planner,'/')+1:]
obs_range = rospy.get_param('/move_base_node/global_costmap/obstacle_range')
move_tol = rospy.get_param('/move_base_node/'+local_planner+'/xy_goal_tolerance')
no_progress = rospy.get_param( rospy.get_name() + '/no_progress_move', 5)
unreached_time = rospy.get_param( rospy.get_name() + '/not_reached_move', 30)
# I'm not entirely sure which service to use. I do know that
# non-NavfnROS make_plan sometimes fails for an unknown
# reason... and thus NavfnROS/make_plan is more robust.
# srv_name = '/move_base_node/make_plan'
srv_name = '/move_base_node/NavfnROS/make_plan'
get_plan = rospy.ServiceProxy( srv_name, GetPlan )
# Clear costmap...? Do this here or in smach...?
if preempt_func(): # immediately exit if overall action preempted
return 'preempted'
for i,w in enumerate( waypoints ):
if preempt_func(): # immediately exit if overall action preempted
return 'preempted'
rospy.logout( 'snaking_explore: Seeking waypoint %d of %d' % (i+1,len(waypoints)))
# rospy.logout( 'snaking_explore: %2.2f %2.2f in frame %s' % (w.pose.position.x, w.pose.position.y, w.header.frame_id))
rim = self.robot_in_map()
w.header.stamp = rospy.Time(0) # transform it _now_
wim = self.ps_in_map( w ) # waypoint in map
if not rim or not wim: # if transforms failed
rospy.logout( 'snaking_explore: Bad transforms. Aborting explore' )
return 'aborted'
if self.range_to_waypoint( rim, wim ) < 0.9 * obs_range:
# We're nearby the waypoint, so we'll just trust that the planner
# has a good view of its surroundings to determine reachability.
req = GetPlanRequest()
req.tolerance = 0.1
req.start = rim
req.goal = wim
resp = get_plan( req )
found_plan = bool( resp.plan.poses != [] )
if not found_plan:
rospy.logout( 'snaking_explore: No plan to nearby waypoint. Proceeding to next waypoint' )
# Perhaps its worth pulling the waypoint closer until a path _is_ found?
continue
# We now know that a path exists. Send the waypoint to the client.
rospy.logout( 'snaking_explore: Near by with good plan. Calling move_base action.' )
else:
# Any nav plan at beyond obstacle range will require the
# nav stack to get a better vantage. Send the waypoint to
# the client.
rospy.logout( 'snaking_explore: Far away. Calling move_base action.' )
# If we made it this far, it's time to call move_base action.
# Cancel any previous goals
self.client.cancel_goals_at_and_before_time( rospy.Time.now() )
if os.environ.has_key('ROBOT') and os.environ['ROBOT'] == 'sim':
rospy.logout( 'running in sim: give the system a little time to respond' )
rospy.sleep( 1 )
# Send our new goal
rospy.logout( 'snaking_explore: sending movebase action goal.' )
move_goal = MoveBaseGoal()
move_goal.target_pose = wim
self.client.send_goal( move_goal )
if os.environ.has_key('ROBOT') and os.environ['ROBOT'] == 'sim':
rospy.logout( 'running in sim: give the system a little time to respond' )
rospy.sleep( 1 )
# We'll monitor the state ourselves, and allow ourselves
# the opportunity to cancel the goal for various reasons
# (eg. if we haven't moved for a while or if new plans
# aren't found after getting within obstacle distance)
# When this loop is broken, we'll go to the next goal. We
# don't need to cancel the goals here -- they will be
# cancelled before a new one is sent.
r = rospy.Rate( 10 )
xytt_old = None # Checks for movement
stime = rospy.Time.now().to_sec()
while True:
if self.client.simple_state == SimpleGoalState.DONE:
res = self.client.get_result()
rospy.logout('snaking_explore: Movebase actionlib completed with result. Proceeding to next waypoint')
break
rim = self.robot_in_map()
w.header.stamp = rospy.Time( 0 )
wim = self.ps_in_map( w )
# Proceed when close enough to goal (within two tolerance factors)
if rim and wim: # only do this check if transform succeeds
if self.range_to_waypoint( rim, wim ) < move_tol * 2.0:
rospy.logout( 'snaking_explore: Close enough to waypoint. Proceeding to next waypoint' )
break # We're near the waypoint, so let's must move on to next
# Proceed when there has been no movement (x,y, or theta) for X sec.
if rim: # only do this check if transform succeeds
xytt = self.xythetatime( rim )
if not xytt_old:
xytt_old = xytt
if np.abs( xytt[0] - xytt_old[0] ) > 0.05 or \
np.abs( xytt[1] - xytt_old[1] ) > 0.05 or \
np.abs( xytt[2] - xytt_old[2] ) > math.radians(10):
xytt_old = xytt
if xytt[3] - xytt_old[3] > no_progress:
rospy.logout( 'snaking_explore: No movement in %2.1f seconds. Proceeding to next waypoint' % no_progress )
break
if rospy.Time.now().to_sec() - stime > unreached_time:
rospy.logout( 'snaking_explore: Goal unreached in %2.1f seconds. Proceeding to next waypoint' % unreached_time )
break
r.sleep()
self.client.cancel_all_goals()
rospy.logout( 'snaking_explore: Finished with exploration.' )
return 'succeeded'
def execute(self, target, blocking=True):
component_name = "base"
ah = ActionHandle("move_base", component_name, target, blocking)
rospy.loginfo("Move <<%s>> to <<%s>>", component_name, target)
#get pose from parameter server
if type(target) is str:
if not rospy.has_param(self.ns_global_prefix + "/" + component_name + "/" + target):
rospy.logerr("parameter %s does not exist on ROS Parameter Server, aborting...", self.ns_global_prefix + "/" + component_name + "/" + target)
ah.set_failed(2)
return ah
param = rospy.get_param(self.ns_global_prefix + "/" + component_name + "/" + target)
else:
param = target
#check pose
if not type(param) is list: # check outer list
rospy.logerr("no valid parameter for %s: not a list, aborting...", component_name)
#print "parameter is:", param
ah.set_failed(3)
return ah
else:
#print i,"type1 = ", type(i)
DOF = 3
if not len(param) == DOF: # check dimension
rospy.logerr("no valid parameter for %s: dimension should be %d and is %d, aborting...", component_name, DOF, len(param))
#print "parameter is:", param
ah.set_failed(3)
return ah
else:
for i in param:
#print i,"type2 = ", type(i)
if not ((type(i) is float) or (type(i) is int)):
#check type
#print type(i)
rospy.logerr("no valid parameter for %s: not a list of float or int, aborting...", component_name)
print "parameter is:", param
ah.set_failed(3)
return ah
else:
rospy.logdebug("accepted parameter %f for %s", i, component_name)
#convert to pose message
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = "/map"
pose.pose.position.x = param[0]
pose.pose.position.y = param[1]
pose.pose.position.z = 0.0
q = tf.transformations.quaternion_from_euler(0, 0, param[2])
pose.pose.orientation.x = q[0]
pose.pose.orientation.y = q[1]
pose.pose.orientation.z = q[2]
pose.pose.orientation.w = q[3]
action_server_name = "/move_base"
rospy.logdebug("calling %s action server", action_server_name)
client = actionlib.SimpleActionClient(action_server_name, MoveBaseAction)
#trying to connect to server
rospy.logdebug("waiting for %s action server to start", action_server_name)
if not client.wait_for_server(rospy.Duration(5)):
#error: server did not respond
rospy.logerr("%s action server not ready within timeout, aborting...", action_server_name)
ah.set_failed(4)
return ah
else:
rospy.logdebug("%s action server ready", action_server_name)
# sending goal
client_goal = MoveBaseGoal()
client_goal.target_pose = pose
print client_goal
client.send_goal(client_goal)
ah.set_client(client)
ah.wait_inside()
return ah
#!/usr/bin/env python
import rospy
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from actionlib import SimpleActionClient
from geometry_msgs.msg import PoseStamped
if __name__ == '__main__':
rospy.init_node('robocup_test_node')
goal = MoveBaseGoal()
pose = PoseStamped()
pose.header.frame_id = 'map'
pose.header.stamp = rospy.Time.now()
pose.pose.position.x = 1.0
pose.pose.position.y = 1.0
pose.pose.orientation.x = 0.0
pose.pose.orientation.y = 0.0
pose.pose.orientation.z = 0.0
pose.pose.orientation.z = 1.0
goal.target_pose = pose
move_base_client = SimpleActionClient('move_base', MoveBaseAction)
move_base_client.send_goal(goal)
move_base_client.wait_for_result(rospy.Duration(20))
