def handle_get_object_types(self, req): res = StringQueryResponse() obj_types = set() for obj in self.scene_objects_dict.values(): # type: SceneObject # obj = self.scene_objects_dict[obj_name] obj_types.update(obj.type) res.output = list(obj_types) res.success = True return res
def handle_get_object_types_by_name(self, req): res = StringQueryResponse() obj_types = [] for obj_name in req.input: obj = self.scene_objects_dict[obj_name] # type: SceneObject obj_types.append(obj.type) res.output = obj_types res.success = True return res
def handle_go_tp_node(self, req): # type: (StringQueryRequest) -> StringQueryResponse res = StringQueryResponse() group, roadmap_name, node = req.input self.load_roadmap(roadmap_name, group) node = int(node) self.ensure_build_plan_requirements() self.move_group_to_roadmap_node(group, node) res.output.append("Moved {} to node {} in {}".format( group, node, self.roadmaps[group])) res.success = True return res
def handle_reload_scene_by_id(self, req): res = StringQueryResponse() scene_id = int(req.input[0]) obj_names = copy.copy(self.scene_objects_dict.keys()) for i in range(self.scene_objects_dict.keys().__len__()): del_objs = self.scene_objects_dict.keys() for k in self.keep_frames: del_objs.remove(k) self.remove_objects(del_objs) # for obj_name in self.scene_objects_dict.keys(): # if obj_name not in self.keep_frames: # self.remove_object(obj_name) # for obj_name in obj_names: # if "_r" not in obj_name: # if self.scene_objects_dict[obj_name].has_visual: # self.remove_object(obj_name) rospy.sleep(1.0) self.delete_planning_scene() self.add_transforms(self.static_transforms) self.scene_object_factory.spawn_scene(self.add_object_from_template, scene_id=scene_id) # self.define_test_scene_graph(scene_id) return res
def handle_del_objects_by_name(self, req): ''' :param req: req.input is a list of object names :return: list of deleted objects ''' res = StringQueryResponse() suc = self.remove_objects(req.input) return res
def handle_get_object_poses_by_names(self, req): res = StringQueryResponse() for obj_name in req.input: s = StringIO.StringIO() self.get_eef_pose_for_grasp(obj_name).serialize(s) s2 = "" for f in s.buflist: s2 += f res.output.append(s2) return res
def handle_get_objects_by_type(self, req): res = StringQueryResponse() for obj_type in req.input: # s = StringIO.StringIO() # self.get_eef_pose_for_grasp(obj_name).serialize(s) objects = self.get_objects(obj_type) for key, obj in objects.items(): obj_str = pickle.dumps(obj) # obj.serialize(s) res.output.append(obj_str) return res
def handle_set_object_color_by_name(self, req): ''' :param req: req.input is a four-tuple: object name, red, green, blue :return: list of deleted objects ''' res = StringQueryResponse() obj_name = req.input[0] r = float(req.input[1]) g = float(req.input[2]) b = float(req.input[3]) # TODO: add test if obj_name in self.scene_objects_dict.keys(): self.scene_objects_dict[obj_name].set_color(r=r, g=g, b=b) return res
def handle_get_object_names_by_type(self, req): res = StringQueryResponse() objs = self.get_objects(req.input) res.output = objs.keys() res.success = True return res