def execute(self, userdata): rospy.loginfo("Wait for task specification from server: " + ip + ":" + port + " (team-name: " + team_name + ")") competitive_task = referee_box_communication.obtainTaskSpecFromServer(ip, port, team_name) #'BTT<(D1,N,6),(S2,E,3)>' rospy.loginfo("Task received: " + competitive_task) # check if Task is a CTT task if(competitive_task[0:3] != "CTT"): rospy.logerr("Excepted <<BTT>> task, but received <<" + competitive_task[0:3] + ">> received") return 'wront_task_format' # remove leading start description competitive_task = competitive_task[3:len(competitive_task)] # check if description has beginning '<' and ending '> if(competitive_task[0] != "<" or competitive_task[(len(competitive_task)-1)] != ">"): rospy.loginfo("task spec not in correct format") return 'wront_task_format' # remove beginning '<' and ending '>' competitive_task = competitive_task[1:len(competitive_task)-1] # Task split task_situation = competitive_task.split(';') rospy.loginfo(task_situation) # Initial Situation initial_situation = task_situation[0] rospy.loginfo(initial_situation) if(initial_situation[0:16] != "initialsituation"): rospy.logerr("Excepted <<initialsituation>>, but received <<" + initial_situation[0:16] + ">> received") return 'wront_task_format' initial_situation = initial_situation[16:len(initial_situation)] rospy.loginfo(initial_situation) init_tasks = re.findall('\<(?P<name>.*?)\>', initial_situation) rospy.loginfo(init_tasks) # Update userdata with expcted initial situation information for item in init_tasks: desired_loc = item[0:2] if item[0] == "D": base_orientation = "W" elif item[0] == "S": base_orientation = "E" obj_taskspec = item[3:len(item)] objs = re.findall('\((?P<name>.*?)\)', obj_taskspec) objs = objs[0].split(',') obj_conf = obj_taskspec.split('(') obj_conf = obj_conf[0] initial_tasklist = Bunch(location=desired_loc,orientation=base_orientation,task='fetch object workspace',object_names=objs,object_config=obj_conf) userdata.task_list.append(initial_tasklist) # Goal Situation goal_situation = task_situation[1] rospy.loginfo(goal_situation) if(goal_situation[0:13] != "goalsituation"): rospy.logerr("Excepted <<goalsituation>>, but received <<" + goal_situation[0:13] + ">> received") return 'wront_task_format' rospy.loginfo(goal_situation) goal_situation = goal_situation[13:len(goal_situation)] rospy.loginfo(goal_situation) goal_tasks = re.findall('\<(?P<name>.*?)\>', goal_situation) rospy.loginfo(goal_tasks) # Update userdata with expcted goal situation information for item in goal_tasks: desired_loc = item[0:2] if item[0] == "D": base_orientation = "W" elif item[0] == "S": base_orientation = "E" obj_taskspec = item[3:len(item)] objs = re.findall('\((?P<name>.*?)\)', obj_taskspec) objs = objs[0].split(',') rospy.loginfo(objs) obj_conf = obj_taskspec.split('(') obj_conf = obj_conf[0] rospy.loginfo(obj_conf) goal_tasklist = Bunch(location=desired_loc,orientation=base_orientation,task='place object in workspace',object_names = objs, object_config = obj_conf) userdata.task_list.append(goal_tasklist) return 'task_received'
def execute(self, userdata): rospy.loginfo("Wait for task specification from server: " + ip + ":" + port + " (team-name: " + team_name + ")") #man_task = "BMT<D2,D2,D2,line(R20,M20_100,F20_20_B),T4>" man_task = referee_box_communication.obtainTaskSpecFromServer(ip, port, team_name) #'BNT<(D1,N,6), rospy.loginfo("Task received: " + man_task) # check if Task is a BNT task if(man_task[0:3] != "BMT"): rospy.logerr("Excepted <<BMT>> task, but received <<" + man_task[0:3] + ">> received") return 'wront_task_format' # remove leading start description man_task = man_task[3:len(man_task)] # check if description has beginning '<' and ending '> if(man_task[0] != "<" or man_task[(len(man_task)-1)] != ">"): rospy.loginfo("task spec not in correct format") return 'wront_task_format' # remove beginning '<' and ending '>' man_task = man_task[1:len(man_task)-1] #print man_task task_list = man_task.split(',') #print task_list init_pose = task_list[0] src_pose = task_list[1] dest_pose = task_list[2] subtask_list = task_list[3].split('(') obj_cfg = subtask_list[0] obj_names = [] obj_names.append(subtask_list[1]) print task_list for i in range(4, (len(task_list)-1)): if i == (len(task_list)-2): print task_list[i] task_list[i] = task_list[i][0:(len(task_list[i])-1)] print task_list[i] obj_names.append(task_list[i]) fnl_pose = task_list[len(task_list)-1] ''' print init_pose print src_pose print dest_pose print obj_cfg print obj_names print fnl_pose ''' for obj in range(len(obj_names)): if obj_names[obj] == "V20": obj_names[obj] = "R20" print obj_names userdata.task_spec = Bunch(inital_pose=init_pose, source_pose=src_pose, destination_pose=dest_pose, object_config=obj_cfg, object_names=obj_names, final_pose=fnl_pose) return 'task_received'
def execute(self, userdata): rospy.loginfo("Wait for task specification from server: " + ip + ":" + port + " (team-name: " + team_name + ")") #transportation_task = 'BTT<initialsituation(<S1,(screw,alu_silver_l)><S2,(alu_silver_s,alu_black_l,alu_black_s)>);goalsituation(<S3,line(alu_silver_l,alu_silver_s)><D1,zigzag(alu_black_s,alu_black_l,screw)>)>' #transportation_task = 'BTT<initialsituation(<S3,(F20_20_B,M20_100)>);goalsituation(<S2,zigzag(M20_100,F20_20_B)>)>' transportation_task = referee_box_communication.obtainTaskSpecFromServer(ip, port, team_name) rospy.loginfo("Task received: " + transportation_task) # check if Task is a BTT task if(transportation_task[0:3] != "BTT"): rospy.logerr("Excepted <<BTT>> task, but received <<" + transportation_task[0:3] + ">> received") return 'wront_task_format' # remove leading start description transportation_task = transportation_task[3:len(transportation_task)] # check if description has beginning '<' and ending '> if(transportation_task[0] != "<" or transportation_task[(len(transportation_task)-1)] != ">"): rospy.loginfo("task spec not in correct format") return 'wront_task_format' # remove beginning '<' and ending '>' transportation_task = transportation_task[1:len(transportation_task)-1] # Task split task_situation = transportation_task.split(';') rospy.loginfo("split1: %s",task_situation) # Initial Situation initial_situation = task_situation[0] rospy.loginfo("init: %s", initial_situation) if(initial_situation[0:16] != "initialsituation"): rospy.logerr("Excepted <<initialsituation>>, but received <<" + initial_situation[0:16] + ">> received") return 'wront_task_format' initial_situation = initial_situation[16:len(initial_situation)] rospy.loginfo('removed <> and (): %s',initial_situation) init_tasks = re.findall('\<(?P<name>.*?)\>', initial_situation) rospy.loginfo("split into poses: %s",init_tasks) # Update userdata with expcted initial situation information for item in init_tasks: desired_loc = item[0:2] obj_taskspec = item[3:len(item)] objs = re.findall('\((?P<name>.*?)\)', obj_taskspec) objs = objs[0].split(',') for i in range(len(objs)): if objs[i] == "V20": objs[i] = "R20" obj_conf = obj_taskspec.split('(') obj_conf = obj_conf[0] rospy.loginfo(" %s %s", desired_loc, objs) initial_tasklist = Bunch(type='source', location = desired_loc, object_names=objs) userdata.task_list.append(initial_tasklist) # Goal Situation goal_situation = task_situation[1] rospy.loginfo('goal %s', goal_situation) if(goal_situation[0:13] != "goalsituation"): rospy.logerr("Excepted <<goalsituation>>, but received <<" + goal_situation[0:13] + ">> received") return 'wront_task_format' goal_situation = goal_situation[13:len(goal_situation)] rospy.loginfo('removed goal string: %s', goal_situation) goal_tasks = re.findall('\<(?P<name>.*?)\>', goal_situation) rospy.loginfo('split into locations: %s', goal_tasks) # Update userdata with expcted goal situation information for item in goal_tasks: desired_loc = item[0:2] obj_taskspec = item[3:len(item)] objs = re.findall('\((?P<name>.*?)\)', obj_taskspec) objs = objs[0].split(',') for i in range(len(objs)): if objs[i] == "V20": objs[i] = "R20" obj_conf = obj_taskspec.split('(') obj_conf = obj_conf[0] rospy.loginfo(" %s %s %s", desired_loc, obj_conf, objs) goal_tasklist = Bunch(type='destination', location = desired_loc, object_names = objs, object_config = obj_conf) userdata.task_list.append(goal_tasklist) return 'task_received'