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