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