def queue_from_db(self, running):
# make sure there's an index on time in the collection so the sort operation doesn't require the whole collection to be loaded
self.collection.ensure_index(TIME_KEY)
# get all documents within the time window, sorted ascending order by time
documents = self.collection.find({TIME_KEY: { '$gte': to_datetime(self.start_time), '$lte': to_datetime(self.end_time)}}, sort=[(TIME_KEY, pymongo.ASCENDING)])
if documents.count() == 0:
rospy.logwarn('No messages to play back from topic %s' % self.collection_name)
return
else:
rospy.logdebug('Playing back %d messages', documents.count())
# load message class for this collection, they should all be the same
msg_cls = mg_util.load_class(documents[0]["_meta"]["stored_class"])
# publisher won't be used until something is on the queue, so it's safe to construct it here
self.publisher = rospy.Publisher(documents[0]["_meta"]["topic"], msg_cls, latch = documents[0]["_meta"]["latch"], queue_size = 10)
for document in documents:
if running.value:
# instantiate the ROS message object from the dictionary retrieved from the db
document['header']['stamp'] = to_ros_time(document['header']['stamp'])
message = mg_util.dictionary_to_message(document, msg_cls)
#print (message, document["header"]["stamp"])
# put will only work while there is space in the queue, if not it will block until another take is performed
self.to_publish.put((message, document["header"]["stamp"]))
else:
break
rospy.logdebug('All messages queued for topic %s' % self.collection_name)
def query_messages_ros_srv(self, req):
"""
Returns t
"""
collection = self._mongo_client[req.database][req.collection]
# build the query doc
obj_query = self.to_query_dict(req.message_query, req.meta_query)
# restrict results to have the type asked for
obj_query["_meta.stored_type"] = req.type
# TODO start using some string constants!
rospy.logdebug("query document: %s", obj_query)
# this is a list of entries in dict format including meta
sort_query_dict = dc_util.string_pair_list_to_dictionary(req.sort_query)
sort_query_tuples = []
for k,v in sort_query_dict.iteritems():
try:
sort_query_tuples.append((k, int(v)))
except ValueError:
sort_query_tuples.append((k,v))
entries = dc_util.query_message(collection, obj_query, sort_query_tuples, req.single, req.limit)
# keep trying clients until we find an answer
for extra_client in self.extra_clients:
if len(entries) == 0:
extra_collection = extra_client[req.database][req.collection]
entries = dc_util.query_message(extra_collection, obj_query, sort_query_tuples, req.single, req.limit)
if len(entries) > 0:
rospy.loginfo("found result in extra datacentre")
else:
break
# rospy.logdebug("entries: %s", entries)
serialised_messages = ()
metas = ()
for entry in entries:
# load the class object for this type
# TODO this should be the same for every item in the list, so could reuse
cls = dc_util.load_class(entry["_meta"]["stored_class"])
# instantiate the ROS message object from the dictionary retrieved from the db
message = dc_util.dictionary_to_message(entry, cls)
# the serialise this object in order to be sent in a generic form
serialised_messages = serialised_messages + (dc_util.serialise_message(message), )
# add ObjectID into meta as it might be useful later
entry["_meta"]["_id"] = entry["_id"]
# serialise meta
metas = metas + (StringPairList([StringPair(dc_srv.MongoQueryMsgRequest.JSON_QUERY, json.dumps(entry["_meta"], default=json_util.default))]), )
return [serialised_messages, metas]
def queue_from_db(self, running):
# make sure there's an index on time in the collection so the sort operation doesn't require the whole collection to be loaded
self.collection.ensure_index(TIME_KEY)
# get all documents within the time window, sorted ascending order by time
documents = self.collection.find(
{
TIME_KEY: {
'$gte': to_datetime(self.start_time),
'$lte': to_datetime(self.end_time)
}
},
sort=[(TIME_KEY, pymongo.ASCENDING)])
if documents.count() == 0:
rospy.logwarn('No messages to play back from topic %s' %
self.collection_name)
return
else:
rospy.logdebug('Playing back %d messages', documents.count())
# load message class for this collection, they should all be the same
msg_cls = mg_util.load_class(documents[0]["_meta"]["stored_class"])
latch = False
if "latch" in documents[0]["_meta"]:
latch = documents[0]["_meta"]["latch"]
# publisher won't be used until something is on the queue, so it's safe to construct it here
self.publisher = rospy.Publisher(documents[0]["_meta"]["topic"],
msg_cls,
latch=latch,
queue_size=10)
for document in documents:
if running.value:
# instantiate the ROS message object from the dictionary retrieved from the db
message = mg_util.dictionary_to_message(document, msg_cls)
# print (message, document["_meta"]["inserted_at"])
# put will only work while there is space in the queue, if not it will block until another take is performed
self.to_publish.put(
(message, to_ros_time(document["_meta"]["inserted_at"])))
else:
break
rospy.logdebug('All messages queued for topic %s' %
self.collection_name)
def query_messages_ros_srv(self, req):
"""
Returns t
"""
collection = self._mongo_client[req.database][req.collection]
# build the query doc
obj_query = self.to_query_dict(req.message_query, req.meta_query)
# restrict results to have the type asked for
obj_query["_meta.stored_type"] = req.type
# TODO start using some string constants!
rospy.logdebug("query document: %s", obj_query)
# this is a list of entries in dict format including meta
sort_query_dict = dc_util.string_pair_list_to_dictionary(
req.sort_query)
sort_query_tuples = []
for k, v in iteritems(sort_query_dict):
try:
sort_query_tuples.append((k, int(v)))
except ValueError:
sort_query_tuples.append((k, v))
# this is a list of entries in dict format including meta
projection_query_dict = dc_util.string_pair_list_to_dictionary(
req.projection_query)
projection_meta_dict = dict()
projection_meta_dict["_meta"] = 1
entries = dc_util.query_message(collection, obj_query,
sort_query_tuples,
projection_query_dict, req.single,
req.limit)
if projection_query_dict:
meta_entries = dc_util.query_message(collection, obj_query,
sort_query_tuples,
projection_meta_dict,
req.single, req.limit)
# keep trying clients until we find an answer
if self.replicate_on_write:
for extra_client in self.extra_clients:
if len(entries) == 0:
extra_collection = extra_client[req.database][
req.collection]
entries = dc_util.query_message(extra_collection,
obj_query,
sort_query_tuples,
projection_query_dict,
req.single, req.limit)
if projection_query_dict:
meta_entries = dc_util.query_message(
extra_collection, obj_query, sort_query_tuples,
projection_meta_dict, req.single, req.limit)
if len(entries) > 0:
rospy.loginfo("found result in extra datacentre")
else:
break
serialised_messages = ()
metas = ()
for idx, entry in enumerate(entries):
# load the class object for this type
# TODO this should be the same for every item in the list, so could reuse
cls = dc_util.load_class(entry["_meta"]["stored_class"])
# instantiate the ROS message object from the dictionary retrieved from the db
message = dc_util.dictionary_to_message(entry, cls)
# the serialise this object in order to be sent in a generic form
serialised_messages = serialised_messages + (
dc_util.serialise_message(message), )
# add ObjectID into meta as it might be useful later
if projection_query_dict:
entry["_meta"]["_id"] = meta_entries[idx]["_id"]
else:
entry["_meta"]["_id"] = entry["_id"]
# serialise meta
metas = metas + (StringPairList([
StringPair(
dc_srv.MongoQueryMsgRequest.JSON_QUERY,
json.dumps(entry["_meta"], default=json_util.default))
]), )
return [serialised_messages, metas]
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
def execute(self, userdata):
rospy.loginfo('State task execution')
# Create action client and goal dynamically. Based on mongodb
action_tuple = self.get_task_types(
userdata.task_struct[0].action_topic)
while not rospy.is_shutdown() and len(action_tuple) == 0:
action_tuple = self.get_task_types(
userdata.task_struct[0].action_topic)
rospy.loginfo("Waiting for action server")
action_clz = dc_util.load_class(
dc_util.type_to_class_string(action_tuple[0]))
rospy.loginfo("Action string %s and goal string %s", action_tuple[0],
action_tuple[1])
# goal_clz = dc_util.load_class(dc_util.type_to_class_string(action_tuple[1]))
# argument_list = self.get_arguments(userdata.task_struct[0].action_arguments)
# mb_goal = goal_clz(*argument_list)
# Create action client and wait for server
userdata.task_struct[1] = actionlib.SimpleActionClient(
userdata.task_struct[0].action_topic, action_clz)
# rospy.loginfo("Waiting for server %s with action class %s", userdata.task_struct[0].action_topic, action_clz)
userdata.task_struct[1].wait_for_server(rospy.Duration(10))
rospy.loginfo("Action server connected!")
# Parse and send the goal to the action server
print userdata.task_struct[0].action_arguments
dictionary = ast.literal_eval(userdata.task_struct[0].action_arguments)
print dictionary
print action_tuple[1]
mb_goal = message_converter.convert_dictionary_to_ros_message(
action_tuple[1], dictionary)
print mb_goal
print mb_goal._type
userdata.task_struct[1].send_goal(mb_goal)
rospy.loginfo("Goal sent!")
# Check periodically state of AUV to preempt goal when needed
t_before = rospy.Time.now()
rate = rospy.Rate(1)
task_result = "running"
while not rospy.is_shutdown() and task_result == "running":
# Check action state from server side
goal_state = userdata.task_struct[1].get_state()
if goal_state == GoalStatus.REJECTED or goal_state == GoalStatus.ABORTED:
task_result = userdata.task_struct[2] = "aborted"
rospy.loginfo("Action aborted!")
break
elif self.preempt_requested(): #self.was_preempted:
task_result = "preempting"
userdata.task_struct[2] = "preempted"
break
elif goal_state == GoalStatus.PREEMPTED or self.preempt_requested(
): #self.was_preempted:
rospy.loginfo("Action preempted!")
task_result = userdata.task_struct[2] = "preempted"
break
elif goal_state == GoalStatus.SUCCEEDED:
rospy.loginfo("Action returned success!")
task_result = userdata.task_struct[2] = "succeeded"
break
# Check duration termination condition if received
if userdata.task_struct[0].max_duration != 0.:
t_after = rospy.Time.now()
if t_after - t_before > userdata.task_struct[0].max_duration:
rospy.loginfo("Time limit reached")
task_result = "preempting"
userdata.task_struct[2] = "preempted"
break
# Check waypoint termination condition if received
if userdata.task_struct[0].x != 0.:
if self.end_condition(userdata):
rospy.loginfo("Success!")
task_result = "preempting"
userdata.task_struct[2] = "succeeded"
break
rate.sleep()
rospy.loginfo("task_result %s", userdata.task_struct[2])
# Result of executing the action
return task_result
#To save depth images without losing values due to png
def save_depth_image(image, filename):
cv.Save(filename, image, name="depth")
#Connecting to the mongodb database.
mong = pymongo.MongoClient("bruxelles", 62345)
db = mong['message_store'].upper_bodies
nimg = db.find({},{"ubd_rgb.encoding":1}).count()
first_doc = True
raw_input("A total of {} images recorded. Press enter to start dumping into {}".format(nimg, os.getcwd()))
for i, entry in enumerate(db.find()):
cls = dc_util.load_class(entry["_meta"]["stored_class"])
message = dc_util.dictionary_to_message(entry, cls)
#print(type(message))
#print(message.robot)
arrayPosition=0
size = len(message.ubd_rgb)
# Dumping the rgb and depth images + adding the images in a json file
if(size!=0):
while(arrayPosition<=size-1):
img_rgb = CvBridge().imgmsg_to_cv2(message.ubd_rgb[arrayPosition])
def execute(self, userdata):
rospy.loginfo('State task execution')
# Create action client and goal dynamically. Based on mongodb
(action_string,
goal_string) = self.get_task_types(userdata.task.action_topic)
action_clz = dc_util.load_class(
dc_util.type_to_class_string(action_string))
rospy.loginfo("Action string %s and goal string %s", action_string,
goal_string)
goal_clz = dc_util.load_class(
dc_util.type_to_class_string(goal_string))
argument_list = self.get_arguments(userdata.task.action_arguments)
mb_goal = goal_clz(*argument_list)
# Create action client and wait for server
self.act_client = actionlib.SimpleActionClient(
userdata.task.action_topic, action_clz)
rospy.loginfo("Waiting for server %s with action class %s",
userdata.task.action_topic, action_clz)
self.act_client.wait_for_server(rospy.Duration(10))
rospy.loginfo("Action server connected!")
# Sends the goal to the action server.
self.act_client.send_goal(mb_goal)
rospy.loginfo("Goal sent!")
# Check periodically state of AUV to preempt goal when needed
t_before = rospy.Time.now()
rate = rospy.Rate(1)
task_result = "running"
while not rospy.is_shutdown() and task_result == "running":
# Check pose of AUV and compare to goal for terminating condition
task_result = self.end_condition(mb_goal)
if task_result == "succeeded":
rospy.loginfo("Success!")
break
# Check state from server side
goal_state = self.act_client.get_state()
if goal_state == GoalStatus.REJECTED or goal_state == GoalStatus.ABORTED:
task_result = "aborted"
rospy.loginfo("Action aborted!")
elif goal_state == GoalStatus.PREEMPTED:
rospy.loginfo("Action preempted!")
task_result = "preempted"
t_after = rospy.Time.now()
if t_after - t_before > userdata.task.max_duration:
rospy.loginfo("Time limit reached")
task_result = "preempted"
break
rate.sleep()
rospy.loginfo("task_result %s", task_result)
if task_result != "succeeded":
self.act_client.cancel_all_goals()
while not rospy.is_shutdown() and goal_state != GoalStatus.PREEMPTED:
goal_state = self.act_client.get_state()
rospy.loginfo("Waiting for server to preempt task")
rospy.Rate(0.5).sleep()
# Result of executing the action
return task_result
def execute_task(self, task):
""" Called to trigger execution of given task. """
# clean up from last execution
self.reset_sm()
rospy.loginfo('Execution of task %s was requested' % task.task_id)
# Create the state machine necessary to execution this task
self.task_sm = smach.StateMachine(
['succeeded', 'aborted', 'preempted'])
self.task_sm.userdata.task = task
with self.task_sm:
# Initialise task data
init_transition = {}
if task.start_node_id != '':
init_transition = {'succeeded': 'TASK_NAVIGATION'}
else:
init_transition = {'succeeded': 'TASK_EXECUTION'}
smach.StateMachine.add('TASK_INITIALISATION',
TaskInitialisation(self),
transitions=init_transition)
# Final task outcomes
smach.StateMachine.add('TASK_SUCCEEDED',
TaskSucceeded(self),
transitions={'succeeded': 'succeeded'})
smach.StateMachine.add('TASK_CANCELLED',
TaskCancelled(self),
transitions={'preempted': 'preempted'})
smach.StateMachine.add('TASK_FAILED',
TaskFailed(self),
transitions={'aborted': 'aborted'})
# when one child terminates, preempt the others
concurrence_child_term_cb = lambda so: True
# navigation action
if task.start_node_id != '':
nav_transitions = {
'preempted': 'TASK_CANCELLED',
'aborted': 'TASK_FAILED'
}
# if no action then nav success leads to task success
if task.action == '':
nav_transitions['succeeded'] = 'TASK_SUCCEEDED'
# else move on to execution
else:
nav_transitions['succeeded'] = 'TASK_EXECUTION'
# create a concurrence which monitors execution time
nav_concurrence = smach.Concurrence(
outcomes=['succeeded', 'preempted', 'aborted'],
default_outcome='preempted',
outcome_cb=self.outcome_cb,
child_termination_cb=concurrence_child_term_cb,
# give states 30 seconds to service a request to shut down
termination_timeout=30)
# register callback for logging
nav_concurrence.register_start_cb(self.nav_start_cb)
nav_concurrence.register_termination_cb(
self.nav_termination_cb)
nav_concurrence.userdata.task = task
with nav_concurrence:
if self.nav_service == BaseTaskExecutor.TOPOLOGICAL_NAV:
from topological_navigation.msg import GotoNodeAction, GotoNodeGoal
# where we want to go
nav_goal = GotoNodeGoal(target=task.start_node_id)
nav_action_clz = GotoNodeAction
nav_action_name = 'topological_navigation'
elif self.nav_service == BaseTaskExecutor.MDP_NAV:
from strands_executive_msgs.msg import ExecutePolicyAction, ExecutePolicyGoal
nav_goal = ExecutePolicyGoal(
task_type=ExecutePolicyGoal.GOTO_WAYPOINT,
target_id=task.start_node_id)
nav_action_clz = ExecutePolicyAction
nav_action_name = 'mdp_plan_exec/execute_policy'
else:
raise RuntimeError('Unknown nav service: %s' %
self.nav_service)
# let nav run for 2.0 times the length it usually takes before terminating
duration_multiplier = 2.0
if rospy.get_param('relaxed_nav', False):
duration_multiplier = 50
# all navigation actions should get at least 30 seconds
monitor_duration = max(
self.expected_navigation_duration_now(
task.start_node_id) * duration_multiplier,
rospy.Duration(30))
smach.Concurrence.add(
'MONITORED',
SimpleActionState(nav_action_name,
nav_action_clz,
goal=nav_goal))
smach.Concurrence.add(
'MONITORING', TimerState(duration=monitor_duration))
smach.StateMachine.add('TASK_NAVIGATION',
nav_concurrence,
transitions=nav_transitions)
# actual task action
if task.action != '':
try:
(action_string,
goal_string) = self.get_task_types(task.action)
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(task.arguments)
goal = goal_clz(*argument_list)
# create a concurrence which monitors execution time along with doing the execution
action_concurrence = smach.Concurrence(
outcomes=['succeeded', 'preempted', 'aborted'],
default_outcome='preempted',
outcome_cb=self.outcome_cb,
child_termination_cb=concurrence_child_term_cb,
# give states 30 seconds to service a request to shut down
termination_timeout=30)
# register callback for logging
action_concurrence.register_start_cb(self.action_start_cb)
action_concurrence.register_termination_cb(
self.action_termination_cb)
action_concurrence.userdata.task = task
with action_concurrence:
smach.Concurrence.add(
'MONITORED',
SimpleActionState(task.action,
action_clz,
goal=goal))
wiggle_room = rospy.Duration(30)
# this prevents the task being interupted if it runs out of time but should still run
def task_can_be_interrupted():
return self.is_task_interruptible(task)
smach.Concurrence.add(
'MONITORING',
TimerState(duration=(task.max_duration +
wiggle_room),
can_finish=task_can_be_interrupted))
smach.StateMachine.add('TASK_EXECUTION',
action_concurrence,
transitions={
'preempted': 'TASK_CANCELLED',
'aborted': 'TASK_FAILED',
'succeeded': 'TASK_SUCCEEDED'
})
except Exception, e:
rospy.logwarn(
'Caught exception when creating the task to execute: %s'
% e)
self.task_failed(task)
return
def to_msg(message):
new_msg = load_class(type_to_class_string(message._type))()
common_attrs = set(dir(message)).intersection(dir(new_msg))
for attr in common_attrs:
if not attr.startswith('_') and not callable(getattr(message, attr)):
setattr(new_msg, attr, getattr(message, attr))
