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 = 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)
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 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 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):
detectionEvents = self.mem.get_and_clear(self.mem.ROS_OBJS_DETECTED)
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 utteranceEvents:
utteranceEvents = []
if not feedback:
feedback = []
for event in detectionEvents:
event.time = midcatime.now()
world.sighting(event)
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)
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 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'])
