def execute(self, ud):
feedback = PerceivePlaneFeedback()
feedback.current_state = 'setup_plane_config'
if 'perceive_plane_goal' in ud:
feedback.message = '[perceive_plane] received plane config goal ' +\
ud.perceive_plane_goal.plane_config
ud.perceive_plane_feedback = feedback
return self.configure_plane(ud.perceive_plane_goal.plane_config)
feedback.message = '[perceive_plane] waiting for plane config goal'
ud.perceive_plane_feedback = feedback
rospy.sleep(self.sleep_duration)
return Constant.WAITING
def execute(self, ud):
if 'perceive_plane_feedback' not in ud:
ud.perceive_plane_feedback = PerceivePlaneFeedback()
ud.perceive_plane_feedback.current_state = 'recognize_objects'
if 'detected_planes' not in ud:
ud.recognized_planes = None
return Constant.FAILURE
for plane in ud.detected_planes.planes:
image_messages = []
for obj in plane.object_list.objects:
image_messages.append(obj.rgb_image)
indices, classes, probs = self._service_proxy.recognize_images(
image_messages)
# TODO: debug output
if len(indices) > 0:
obj_index = indices[0]
self._image_pub.publish(image_messages[obj_index])
rospy.loginfo('first object found: {0} (prob: {1})'.format(
classes[obj_index], probs[obj_index]))
else:
rospy.logwarn('no object recognized for plane ' + plane.name)
for i in indices:
plane.object_list.objects[i].name = classes[i]
# TODO: handle categories
plane.object_list.objects[i].category = classes[i]
ud.recognized_planes = ud.detected_planes
return Constant.SUCCESS
def execute(self, ud):
if 'perceive_plane_feedback' not in ud:
ud.perceive_plane_feedback = PerceivePlaneFeedback()
ud.perceive_plane_feedback.current_state = 'detect_objects'
self._detecting_done = False
ud.detected_planes = None
if 'perceive_plane_goal' not in ud:
rospy.logerr('no goal passed into DetectObjects state')
return Constant.FAILURE
self._detector.start_detect_objects(
ud.perceive_plane_goal.plane_frame_prefix, self._detection_cb,
self._target_frame)
timeout = rospy.Duration.from_sec(self._timeout_duration)
rate = rospy.Rate(10) # 10Hz
start_time = rospy.Time.now()
while (rospy.Time.now() - start_time) < timeout:
if self._detecting_done:
if self._detector.plane_list is None:
return Constant.FAILURE
ud.detected_planes = self._detector.plane_list
return Constant.SUCCESS
rate.sleep()
return Constant.TIMEOUT
def execute(self, ud):
if 'perceive_plane_feedback' not in ud:
ud.perceive_plane_feedback = PerceivePlaneFeedback()
ud.perceive_plane_feedback.current_state = 'set_action_lib_result'
result = PerceivePlaneResult()
result.success = self.result
if ud.recognized_planes is None:
result.recognized_planes = PlaneList()
result.recognized_planes.planes = []
else:
result.recognized_planes = ud.recognized_planes
ud.perceive_plane_result = result
return Constant.SUCCESS