def publish_goal(self, x=0, y=0, z=0, yaw=0, roll=0, pitch=0):
# Create move_base goal
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = rospy.get_param('~frame_id', 'map')
goal.target_pose.pose.position.x = x
goal.target_pose.pose.position.y = y
goal.target_pose.pose.position.z = z
quaternion = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
goal.target_pose.pose.orientation.x = quaternion[0]
goal.target_pose.pose.orientation.y = quaternion[1]
goal.target_pose.pose.orientation.z = quaternion[2]
goal.target_pose.pose.orientation.w = quaternion[3]
rospy.loginfo(
'Executing move_base goal to position (x,y) %s, %s, with %s degrees yaw.'
% (x, y, yaw))
rospy.loginfo(
"To cancel the goal: 'rostopic pub -1 /move_base/cancel actionlib_msgs/GoalID -- {}'"
)
# Send goal
self.move_base.send_goal(goal)
rospy.loginfo('Inital goal status: %s' %
GoalStatus.to_string(self.move_base.get_state()))
status = self.move_base.get_goal_status_text()
if status:
rospy.loginfo(status)
# Wait for goal result
self.move_base.wait_for_result()
rospy.loginfo('Final goal status: %s' %
GoalStatus.to_string(self.move_base.get_state()))
status = self.move_base.get_goal_status_text()
if status:
rospy.loginfo(status)
def navigation_goal_to(self, x, y):
# Create move_base goal
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = 'map'
goal.target_pose.pose.position.x = x
goal.target_pose.pose.position.y = y
goal.target_pose.pose.position.z = 0.0
goal.target_pose.pose.orientation.x = 0.0
goal.target_pose.pose.orientation.y = 0.0
goal.target_pose.pose.orientation.z = 0.0
goal.target_pose.pose.orientation.w = 1
rospy.loginfo('Executing move_base goal to position (x,y) %s, %s.' %
(goal.target_pose.pose.position.x,
goal.target_pose.pose.position.y))
rospy.loginfo(
"To cancel the goal: 'rostopic pub -1 /move_base/cancel actionlib_msgs/GoalID -- {}'"
)
# Send goal
self.move_base.send_goal(goal)
rospy.loginfo('Inital goal status: %s' %
GoalStatus.to_string(self.move_base.get_state()))
status = self.move_base.get_goal_status_text()
if status:
rospy.loginfo(status)
# Wait for goal result
self.move_base.wait_for_result()
rospy.loginfo('Final goal status: %s' %
GoalStatus.to_string(self.move_base.get_state()))
status = self.move_base.get_goal_status_text()
if status:
rospy.loginfo(status)
def start_job(self, job):
result = TransportResult()
try:
goal = job.get_goal()
color = goal.specification
item = self.warehouse.get(color)()
if item.available:
rospy.loginfo( 'Call stacker for job: ' + str([item.x, item.z, goal.address, 1]) )
self.current_gh = self.stacker.send_goal( StackerGoal([item.x, item.z, goal.address, 1]) )
while not rospy.is_shutdown(): # wait for stacker complete its job
goal_status = self.current_gh.get_goal_status()
rospy.logdebug( 'Goal status: ' + str(goal_status) )
if goal_status > 2: # actionlib_msgs/GoalStatus
rospy.loginfo( 'Goal status: ' + str(goal_status) )
break
rospy.sleep(1)
result.act = 'Job %s %s' % ( str(job.get_goal_id()), GoalStatus.to_string(self.current_gh.get_terminal_state()) )
job.set_succeeded(result)
else:
result.act = 'Spec %s is not available' % color
job.set_aborted(result)
rospy.logwarn('Job aborted')
except Exception:
rospy.logwarn('Exception arise!')
finally:
rospy.loginfo('Finishing')
self.finish()
self.make_bids()
self.busy = False
def done_cb(self, state, r):
rospy.loginfo(
gstr({
'done state': GoalStatus.to_string(state),
'elapsed time': time.time() - self.start,
'loss': r.loss
}))
if state == GoalStatus.SUCCEEDED:
self.queue.put(r)
def execute_policy_cb(self, goal):
self.cancelled = False
self.policy_mdp = self.policy_utils.generate_policy_mdp(
goal.spec, self.closest_waypoint, rospy.Time.now())
if self.policy_mdp is None:
rospy.logerr("Failure to build policy for specification: " +
goal.spec.ltl_task)
self.mdp_as.set_aborted()
return
self.publish_feedback(None, None, self.policy_mdp.get_current_action())
if self.nav_before_action_exec:
starting_exec = True #used to make sure the robot executes calls topological navigation at least once before executing non-nav actions. This is too ensure the robot navigates to the exact pose of a waypoint before executing an action there
else:
starting_exec = False
while (self.policy_mdp.has_action_defined()
and not self.cancelled) or starting_exec:
next_action = self.policy_mdp.get_current_action()
if next_action in self.mdp.nav_actions or starting_exec:
starting_exec = False
current_nav_policy = self.policy_utils.generate_current_nav_policy(
self.policy_mdp)
status = self.execute_nav_policy(current_nav_policy)
rospy.loginfo(
"Topological navigation execute policy action server exited with status: "
+ GoalStatus.to_string(status))
if status != GoalStatus.SUCCEEDED:
self.policy_mdp.set_current_state(None)
break
else:
print("EXECUTE ACTION")
(status, state_update) = self.action_executor.execute_action(
self.mdp.action_descriptions[next_action])
executed_action = next_action
print(executed_action)
if not self.cancelled:
next_flat_state = self.policy_utils.get_next_state_from_action_outcome(
state_update, self.policy_mdp)
self.policy_mdp.set_current_state(next_flat_state)
if next_flat_state is None:
rospy.logerr(
"Error finding next state after action execution. Aborting..."
)
break
next_action = self.policy_mdp.get_current_action()
self.publish_feedback(executed_action, status, next_action)
if self.cancelled:
self.cancelled = False
rospy.loginfo("Policy execution preempted.")
self.mdp_as.set_preempted()
elif self.policy_mdp.current_flat_state is None:
rospy.loginfo("Policy execution failed.")
self.mdp_as.set_aborted()
else:
rospy.loginfo("Policy execution successful.")
self.mdp_as.set_succeeded()
def main():
rospy.init_node("autonomy")
client = actionlib.SimpleActionClient("move_base", MoveBaseAction)
rospy.loginfo("Waiting for robot action server")
client.wait_for_server()
rospy.loginfo("Server found")
while True:
failure = True
while failure:
try:
x = float(
input("Enter x-coordinate of where you want to go\n"))
y = float(
input("Enter y-coordinate of where you want to go\n"))
input("Hit <Enter> when you are ready")
failure = False
except ValueError:
print("Oops enter a valid value\n")
failure = True
rospy.loginfo("Sent")
action_goal = MoveBaseActionGoal()
action_goal.goal.target_pose.header.frame_id = "map"
action_goal.goal.target_pose.pose.position.x = x
action_goal.goal.target_pose.pose.position.y = y
action_goal.goal.target_pose.pose.orientation.w = 1
client.send_goal(action_goal.goal)
input("Press <Enter> to stop robot or continue")
if client.get_state() == GoalStatus.ACTIVE:
rospy.loginfo("Canceling action")
client.cancel_goal()
try:
print("result: ", GoalStatus.to_string(client.get_state()))
except Exception as e:
print(e)
while not rospy.is_shutdown():
rospy.spin()
def update_status(self, status_array):
with self.mutex:
if self.state == CommState.DONE:
return
status = _find_status_by_goal_id(status_array,
self.action_goal.goal_id.id)
# You mean you haven't heard of me?
if not status:
if self.state not in [
CommState.WAITING_FOR_GOAL_ACK,
CommState.WAITING_FOR_RESULT, CommState.DONE
]:
self._mark_as_lost()
return
self.latest_goal_status = status
# Determines the next state from the lookup table
if self.state not in _transitions:
rospy.logerr("CommStateMachine is in a funny state: %i" %
self.state)
return
if status.status not in _transitions[self.state]:
rospy.logerr(
"Got an unknown status from the ActionServer: %i" %
status.status)
return
next_state = _transitions[self.state][status.status]
# Knowing the next state, what should we do?
if next_state == NO_TRANSITION:
pass
elif next_state == INVALID_TRANSITION:
rospy.logerr("Invalid goal status transition from %s to %s" %
(CommState.to_string(self.state),
GoalStatus.to_string(status.status)))
else:
if hasattr(next_state, '__getitem__'):
for s in next_state:
self.transition_to(s)
else:
self.transition_to(next_state)
def start_job(self, job):
rospy.logdebug('Storage.start_job: ' + str(job))
result = TransportResult()
try:
goal = job.get_goal()
color = goal.specification
cell = self.warehouse.get(color)()
if cell.available:
self.conveyor_destination('warehouse')
self.conveyor_load(
goal.address
) # plant must already wait for low level unload premission
rospy.logdebug('Address: ' + str(goal.address))
while not rospy.is_shutdown():
product = self.conveyor_product_ready()
if product.ready:
break
rospy.sleep(1)
self.current_gh = self.stacker.send_goal(
StackerGoal([12, 1, cell.x,
cell.z])) # [12, 1] is conveyor
while not rospy.is_shutdown(
): # wait for stacker complete its job
goal_status = self.current_gh.get_goal_status()
if goal_status > 2: # actionlib_msgs/GoalStatus
break
rospy.logdebug('Stacker goal status: ' + str(goal_status))
rospy.sleep(1)
result.act = 'Job %s %s' % (
str(job.get_goal_id()),
GoalStatus.to_string(self.current_gh.get_terminal_state()))
job.set_succeeded(result)
else:
self.conveyor_destination('garbage')
self.conveyor_load(goal.address)
result.act = 'Place for %s is not available. Throwing to garbage.' % color
rospy.sleep(10) # wait until chunk throwed down
job.set_succeeded(result)
finally:
self.finish()
self.make_bids()
self.busy = False
def _send_command(self, client, goal, blocking=False, timeout=None):
if client not in self.clients:
return False
self.clients[client].send_goal(goal)
start_time = rospy.Time.now()
# rospy.loginfo(goal)
rospy.sleep(0.1)
if self.COMMAND_LOGS:
rospy.loginfo("Command sent to %s client" % client)
if not blocking: # XXX why isn't this perfect?
return None
status = 0
end_time = rospy.Time.now() + rospy.Duration(timeout + 0.1)
while (not rospy.is_shutdown()) and \
(rospy.Time.now() < end_time) and \
(status < GoalStatus.SUCCEEDED) and \
(type(self.clients[client].action_client.last_status_msg) != type(None)):
status = self.clients[
client].action_client.last_status_msg.status_list[
-1].status # XXX get to 80
self.rate.sleep()
# It's reporting time outs that are too early
# FIXED: See comments about rospy.Time(0)
text = GoalStatus.to_string(status)
if GoalStatus.SUCCEEDED <= status:
if self.COMMAND_LOGS:
rospy.loginfo("Goal status {} achieved. Exiting".format(text))
else:
if self.ERROR_LOGS:
rospy.loginfo("Ending due to timeout {}".format(text))
result = self.clients[client].get_result()
elapsed_time = (rospy.Time.now() - start_time).to_sec()
print('Executed in {:.3f} seconds. Predicted to take {:.3f} seconds.'.
format(elapsed_time, timeout))
#state = self.clients[client].get_state()
#print('Goal state {}'.format(GoalStatus.to_string(state)))
# get_goal_status_text
return result
def __done_callback__(self, state, result):
"""
Callback when the execution of __goto_waypoint__ has finished
We check if the execution was successful and trigger the next command
"""
if state == GoalStatus.SUCCEEDED:
# Publish stop message and reduce number of random waypoints
rospy.loginfo('Reached waypoint')
self.stop_pub.publish(Empty())
# Sample was valid, so reduce count by one
if self.random_waypoint_number > 0:
self.random_waypoint_number -= 1
else:
# Execution was not successful, so abort execution and reset the simulation
rospy.loginfo('Action returned: {}'.format(
GoalStatus.to_string(state)))
self.abort_pub.publish(Empty())
self.__reset_simulation__()
if self.random_waypoint_number > 0:
rospy.loginfo('Resample this random waypoint')
# Wait shortly before publishing the next command
rospy.sleep(0.5)
if self.random_waypoint_number > 0:
# Their are still random waypoints to generate
item = self.mission[self.mission_index]
self.__goto_random__(item[1])
else:
# Previous command has finished, so go to the next command in the list
self.mission_index += 1
self.__send_next_command__()
def do_gps_goal(lat, long, marker_only=False):
rospy.init_node('gps_goal')
# Check for degrees, minutes, seconds format and convert to decimal
if ',' in lat:
degrees, minutes, seconds = lat.split(',')
degrees, minutes, seconds = float(degrees), float(minutes), float(
seconds)
if lat[0] == '-': # check for negative sign
minutes = -minutes
seconds = -seconds
lat = degrees + minutes / 60 + seconds / 3600
if ',' in long:
degrees, minutes, seconds = long.split(',')
degrees, minutes, seconds = float(degrees), float(minutes), float(
seconds)
if long[0] == '-': # check for negative sign
minutes = -minutes
seconds = -seconds
long = degrees + minutes / 60 + seconds / 3600
goal_lat = float(lat)
goal_long = float(long)
rospy.loginfo('Given GPS goal: lat %s, long %s.' % (goal_lat, goal_long))
# Get the lat long coordinates of our map frame's origin which must be publshed on topic /local_xy_origin. We use this to calculate our goal within the map frame.
origin_topic = '/local_xy_origin'
rospy.loginfo('Listening for origin location on topic %s' % origin_topic)
origin_pose = rospy.wait_for_message(origin_topic, PoseStamped, timeout=10)
origin_lat = origin_pose.pose.position.y
origin_long = origin_pose.pose.position.x
rospy.loginfo('Received origin: lat %s, long %s.' %
(origin_lat, origin_long))
# Calculate distance and azimuth between GPS points
geod = Geodesic.WGS84 # define the WGS84 ellipsoid
g = geod.Inverse(
origin_lat, origin_long, goal_lat, goal_long
) # Compute several geodesic calculations between two GPS points
hypotenuse = distance = g['s12'] # access distance
rospy.loginfo(
"The distance from the origin to the goal is {:.3f} m.".format(
distance))
azimuth = g['azi1']
rospy.loginfo(
"The azimuth from the origin to the goal is {:.3f} degrees.".format(
azimuth))
# Convert polar (distance and azimuth) to x,y translation in meters (needed for ROS) by finding side lenghs of a right-angle triangle
x = adjacent = math.cos(azimuth) * hypotenuse
y = opposite = math.sin(azimuth) * hypotenuse
rospy.loginfo(
"The translation from the origin to the goal is (x,y) {:.3f}, {:.3f} m."
.format(x, y))
# TODO(Dan): Set target yaw pose
# import tf_conversions
# if yaw:
# euler_tuple = tf_conversions.transformations.euler_from_quaternion(quaternion)
# else:
# # Use azimuth as yaw
# Publish goal as point message for visualization purposes
marker_topic = 'point_to_marker' # publish GPS point in ROS coordinates for visualization
point_publisher = rospy.Publisher(marker_topic,
PointStamped,
queue_size=10)
rospy.sleep(1) # allow time for subscribers to join
gps_point = PointStamped()
gps_point.point.x = x
gps_point.point.y = y
gps_point.point.z = 0
point_publisher.publish(gps_point)
rospy.loginfo("Published point {:.3f}, {:.3f} on topic {}.".format(
x, y, marker_topic))
if marker_only:
return
# Create move_base goal
rospy.loginfo("Connecting to move_base...")
move_base = actionlib.SimpleActionClient('move_base', MoveBaseAction)
move_base.wait_for_server()
rospy.loginfo("Connected.")
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = 'map'
goal.target_pose.pose.position.x = x
goal.target_pose.pose.position.y = y
goal.target_pose.pose.position.z = 0.0
goal.target_pose.pose.orientation.x = 0.0
goal.target_pose.pose.orientation.y = 0.0
goal.target_pose.pose.orientation.z = 0.0
goal.target_pose.pose.orientation.w = 1
rospy.loginfo(
'Executing move_base goal to position (x,y) %s, %s.' %
(goal.target_pose.pose.position.x, goal.target_pose.pose.position.y))
rospy.loginfo(
"To cancel the goal: 'rostopic pub -1 /move_base/cancel actionlib_msgs/GoalID -- {}'"
)
# Send goal
move_base.send_goal(goal)
rospy.loginfo('Inital goal status: %s' %
GoalStatus.to_string(move_base.get_state()))
status = move_base.get_goal_status_text()
if status:
rospy.loginfo(status)
# Wait for goal result
move_base.wait_for_result()
rospy.loginfo('Final goal status: %s' %
GoalStatus.to_string(move_base.get_state()))
status = move_base.get_goal_status_text()
if status:
rospy.loginfo(status)
def print_status(line_limit=20):
if all_tasks_data is None or active_tasks_data is None:
return
active_ids = set(t.task_id for t in active_tasks_data.execution_queue)
non_active_tasks = [
t for t in all_tasks_data.execution_queue
if not t.task_id in active_ids
]
active_tasks = active_tasks_data.execution_queue
line_count = 0
rospy.loginfo("")
rospy.loginfo("")
rospy.loginfo(
datetime.fromtimestamp(rospy.get_rostime().secs).replace(
tzinfo=localtz).strftime("%H:%M:%S %d/%m/%y"))
line_count += 1
if len(active_tasks) > 0:
rospy.loginfo("Current tasks:")
line_count += 1
for t in active_tasks:
rospy.loginfo(" %s" % pretty(t))
line_count += 1
if goal_string is not None:
rospy.loginfo("Policy:")
rospy.loginfo(' as LTL string %s' % goal_string)
line_count += 2
if feedback_data is not None:
rospy.loginfo(' executed action: %s, status %s' %
(feedback_data.feedback.executed_action,
GoalStatus.to_string(
feedback_data.feedback.execution_status)))
rospy.loginfo(' next action: %s' %
(feedback_data.feedback.next_action))
rospy.loginfo(
' policy completing with probability %d, expected time %ds' %
(feedback_data.feedback.probability,
feedback_data.feedback.expected_time.to_sec()))
line_count += 3
rospy.loginfo("Batch started at %s", start_time(active_tasks[0]))
line_count += 1
rospy.loginfo(" finish by %s", end_time(active_tasks[0]))
line_count += 1
else:
rospy.loginfo("No active tasks")
line_count += 1
if len(non_active_tasks) > 0:
rospy.loginfo("Future tasks (in some order):")
line_count += 1
printed = 0
for t in non_active_tasks:
if line_count > line_limit - 2:
rospy.loginfo(" ... and another %s tasks not printed" %
(len(non_active_tasks) - printed))
return
else:
rospy.loginfo(
" %s after %s" %
(pretty(t), rostime_to_python(
t.start_after,
tz=localtz).strftime("%H:%M:%S %d/%m/%y")))
line_count += 1
printed += 1
else:
rospy.loginfo("No additional tasks")
def dump_result_cb(self, userdata, status, result):
rospy.loginfo("DUMP state returned with status: " +
GoalStatus.to_string(status))
userdata.direction_out = 'dig'
userdata.sub_run += 1
return 'succeeded'
def dig_result_cb(self, userdata, status, result):
rospy.loginfo("DIG state returned with status: " +
GoalStatus.to_string(status))
userdata.direction_out = 'dump'
return 'succeeded'
def drive_result_cb(self, userdata, status, result):
rospy.loginfo("DRIVE state returned with status: " +
GoalStatus.to_string(status))
if status == GoalStatus.SUCCEEDED:
return 'succeeded_dig' if userdata.direction_in == 'dig' else 'succeeded_dump'
def execute_action(self, action_msg):
self.cancelled = False
rospy.loginfo("Executing " + action_msg.name +
". Actionlib server is: " + action_msg.action_server)
if action_msg.action_server != '':
try:
(action_string,
goal_string) = self.get_action_types(action_msg.action_server)
action_clz = dc_util.load_class(
dc_util.type_to_class_string(action_string))
goal_clz = dc_util.load_class(
dc_util.type_to_class_string(goal_string))
argument_list = self.get_arguments(action_msg.arguments)
goal = goal_clz(*argument_list)
poll_wait = rospy.Duration(1)
if action_msg.max_duration == rospy.Duration(0):
max_action_duration = self.get_max_action_duration(
action_msg.outcomes)
else:
max_action_duration = action_msg.max_duration.to_sec()
wiggle_room = 30
action_client = actionlib.SimpleActionClient(
action_msg.action_server, action_clz)
action_client.wait_for_server(poll_wait)
action_client.send_goal(goal)
action_finished = False
timer = 0
while (not action_finished) and (not self.cancelled):
timer += poll_wait.to_sec()
action_finished = action_client.wait_for_result(poll_wait)
if timer > (max_action_duration + wiggle_room):
if self.is_action_interruptible(
action_msg.action_server):
break
else:
rospy.logwarn(
'Action execution has timed out but is not interruptible, so execution continues'
)
if not action_finished:
if self.cancelled:
rospy.logwarn(
"Policy execution has been preempted by another process"
)
else:
rospy.logwarn(
'Action %s exceeded maximum duration, preempting' %
action_msg.name)
action_client.cancel_all_goals()
action_finished = action_client.wait_for_result(
rospy.Duration(
60)) #give some time for action to cancel
if not action_finished:
rospy.logwarn(
'Action %s did not respond to preemption, carrying on regardless. This could have dangerous side effects.'
% action_msg.name)
status = action_client.get_state()
result = action_client.get_result()
print GoalStatus.to_string(status)
print result
except Exception, e:
rospy.logwarn(
'Caught exception when trying to execute the action: %s' %
e)
status = GoalStatus.ABORTED
result = None