def fake_point(data):
print("pretending to point")
msg = data.data
d = rosrun.msg_as_dict(msg)
time.sleep(2)
print("sending feedback")
midca_feedback(cmd_id=d['cmd_id'], code=asynch.COMPLETE)
def run(self, cycle, verbose=2):
#self.ObserveWorld()
detectionEvents = self.mem.get_and_clear(self.mem.ROS_OBJS_DETECTED)
detecttionBlockState = self.mem.get_and_clear(self.mem.ROS_OBJS_STATE)
utteranceEvents = self.mem.get_and_clear(self.mem.ROS_WORDS_HEARD)
feedback = self.mem.get_and_clear(self.mem.ROS_FEEDBACK)
world = self.mem.get_and_lock(self.mem.STATE)
if not detectionEvents:
detectionEvents = []
if not detecttionBlockState:
detecttionBlockState = []
if not utteranceEvents:
utteranceEvents = []
if not feedback:
feedback = []
for event in detectionEvents:
event.time = time.now()
world.sighting(event)
for blockstate in detecttionBlockState:
blockstate.time = time.now()
world.position(blockstate)
for event in utteranceEvents:
event.time = time.now()
world.utterance(event)
for msg in feedback:
d = rosrun.msg_as_dict(msg)
d['received_at'] = float(time.now())
self.mem.add(self.mem.FEEDBACK, d)
self.mem.unlock(self.mem.STATE)
if verbose > 1:
print "World observed:", len(
detectionEvents), "new detection event(s),", len(
utteranceEvents), "utterance(s) and", len(
feedback), "feedback msg(s)"
def fake_point(data):
print("pretending to point")
msg = data.data
d = rosrun.msg_as_dict(msg)
time.sleep(2)
print("sending feedback")
midca_feedback(cmd_id = d['cmd_id'], code = asynch.COMPLETE)
def run(self, cycle, verbose = 2): #self.ObserveWorld() detectionEvents = self.mem.get_and_clear(self.mem.ROS_OBJS_DETECTED) detecttionBlockState = self.mem.get_and_clear(self.mem.ROS_OBJS_STATE) utteranceEvents = self.mem.get_and_clear(self.mem.ROS_WORDS_HEARD) feedback = self.mem.get_and_clear(self.mem.ROS_FEEDBACK) world = self.mem.get_and_lock(self.mem.STATE) if not detectionEvents: detectionEvents = [] if not detecttionBlockState: detecttionBlockState = [] if not utteranceEvents: utteranceEvents = [] if not feedback: feedback = [] for event in detectionEvents: event.time = time.now() world.sighting(event) for blockstate in detecttionBlockState: blockstate.time = time.now() world.position(blockstate) for event in utteranceEvents: event.time = time.now() world.utterance(event) for msg in feedback: d = rosrun.msg_as_dict(msg) d['received_at'] = float(time.now()) self.mem.add(self.mem.FEEDBACK, d) self.mem.unlock(self.mem.STATE) if verbose > 1: print "World observed:", len(detectionEvents), "new detection event(s),", len(utteranceEvents), "utterance(s) and", len(feedback), "feedback msg(s)"
def msg_callback(data):
print("got msg:", data)
msg = data.data
d = rosrun.msg_as_dict(msg)
if not 'x' in d and 'y' in d and 'z' in d and 'cmd_id' in d:
rospy.logerr("Msg received by pointing node, but it does not contain" +
"keys 'x', 'y', 'z', 'cmd_id'. Msg will be ignored.")
midca_feedback(cmd_id = d['cmd_id'], code = asynch.FAILED)
else:
try:
point = Point(x = int(d['x']), y = int(d['y']), z = int(d['z']))
except NumberFormatException:
rospy.logerr("Msg received by pointing nodes, but target coordinates" +
" are not well-formed. Msg will be ignored.")
midca_feedback(cmd_id = d['cmd_id'], code = asynch.FAILED)
return
point_callback(point, d['cmd_id'])
def run(self, cycle, verbose=2):
#self.ObserveWorld()
detectionEvents = self.mem.get_and_clear(self.mem.ROS_OBJS_DETECTED)
detecttionBlockState = self.mem.get_and_clear(self.mem.ROS_OBJS_STATE)
utteranceEvents = self.mem.get_and_clear(self.mem.ROS_WORDS_HEARD)
feedback = self.mem.get_and_clear(self.mem.ROS_FEEDBACK)
world = self.mem.get_and_lock(self.mem.STATE)
history = self.mem.get_and_lock(self.mem.STATE_HISTORY)
if not detectionEvents:
detectionEvents = []
if not detecttionBlockState:
detecttionBlockState = []
if not utteranceEvents:
utteranceEvents = []
if not feedback:
feedback = []
for event in detectionEvents:
event.time = midcatime.now()
world.sighting(event)
for blockstate in detecttionBlockState:
blockstate.time = midcatime.now()
world.position(blockstate)
for event in utteranceEvents:
event.time = midcatime.now()
world.utterance(event)
for msg in feedback:
d = rosrun.msg_as_dict(msg)
d['received_at'] = float(midcatime.now())
self.mem.add(self.mem.FEEDBACK, d)
# if there are any change in events remember
history = self.check_with_history(world, history, detecttionBlockState)
self.mem.unlock(self.mem.STATE_HISTORY)
if history:
if len(history) > 5:
history = history[:5]
self.mem.set(self.mem.STATE_HISTORY, history)
self.mem.unlock(self.mem.STATE)
if verbose > 1:
print "World observed:", len(
detectionEvents), "new detection event(s),", len(
utteranceEvents), "utterance(s) and", len(
feedback), "feedback msg(s)"
def msg_callback(data):
print("got msg:", data)
msg = data.data
d = rosrun.msg_as_dict(msg)
if not 'x' in d and 'y' in d and 'z' in d and 'cmd_id' in d:
rospy.logerr("Msg received by pointing node, but it does not contain" +
"keys 'x', 'y', 'z', 'cmd_id'. Msg will be ignored.")
midca_feedback(cmd_id=d['cmd_id'], code=asynch.FAILED)
else:
try:
point = Point(x=int(d['x']), y=int(d['y']), z=int(d['z']))
except NumberFormatException:
rospy.logerr(
"Msg received by pointing nodes, but target coordinates" +
" are not well-formed. Msg will be ignored.")
midca_feedback(cmd_id=d['cmd_id'], code=asynch.FAILED)
return
point_callback(point, d['cmd_id'])
def grabbing_callback(data):
print("got msg:", data)
msg = data.data
d = rosrun.msg_as_dict(msg)
msg_callback(msg, d['cmd_id'])
def releasing_callback(data):
print("got msg:", data)
msg = data.data
d = rosrun.msg_as_dict(msg)
release_callback(msg, d['cmd_id'])
def grabbing_callback(data):
print("got msg:", data)
msg = data.data
d = rosrun.msg_as_dict(msg)
msg_callback(msg, d['cmd_id'])
def releasing_callback(data):
print("got msg:", data)
msg = data.data
d = rosrun.msg_as_dict(msg)
release_callback(msg, d['cmd_id'])
