def check_if_turn(userdata):
if userdata.in_times_turned >= times_to_turn:
if not userdata.in_furniture_poses: # No furniture has been detected
return 'no_furniture'
if publish_markers: # We publish the markers
publish_markerArray(publisher, points=userdata.in_furniture_poses, rgba=(0, 1, 0, 1), shape=Marker.CUBE)
return succeeded
userdata.out_times_turned = userdata.in_times_turned + 1
return 'turn_again'
def calculate_tabletop_sensing_locations_route(userdata):
params = rospy.get_param(PARAM_CORNER_NAME+userdata.in_room_name, None)
if params is None:
return 'no_corner_info'
# Note that the ServiceState with the response slots returns in the userdata the response.map variable.
visib = Visib_PRM(userdata.in_map, params[0], params[1], params[2], params[3], params[4])
(guards, connection) = visib.visib_prm(in_mtrs=True)
userdata.out_guards = guards
# FIXME consider deleting the visib thing
if publish_markers:
publish_markerArray(publisher, points=guards, rgba=(1.0, 0.0, 0.0, 1.0), shape=Marker.CYLINDER)
return succeeded
