def get_object_instances_list(req):
res = GetObjectInstancesListResponse()
objects = db().query(ObjectInstance.id, ObjectInstance.name, ObjectInstance.alias, FrameNode).filter(ObjectInstance.frame_id == FrameNode.id).all()
for id, name, alias, frame in objects:
obj = ROSObjectInstance()
obj.id = id
obj.name = name
obj.alias = alias
obj.pose = frame.toROSPoseStamped()
res.objects.append(obj)
return res
def get_object_instances_list( req ):
rospy.loginfo( "DB SRVs: get_object_instances_list" )
res = GetObjectInstancesListResponse()
objects = db().query( ObjectInstance.id, ObjectInstance.name, ObjectInstance.alias, FrameNode, ObjectDescription.type).\
filter( ObjectInstance.id.in_( any_obj_types_ids(ObjectInstance, req.types) ), ObjectInstance.relative_description_id == ObjectDescription.id, ObjectInstance.frame_id == FrameNode.id ).all()
for id, name, alias, frame, type in objects:
obj = ROSObjectInstance()
obj.id = id
obj.name = name
obj.alias = alias
obj.pose = frame.toROSPoseStamped()
obj.description.type = type
res.objects.append( obj )
return res
def get_object_instances_list(req):
rospy.loginfo("DB SRVs: get_object_instances_list")
res = GetObjectInstancesListResponse()
objects = db().query( ObjectInstance.id, ObjectInstance.name, ObjectInstance.alias, FrameNode, ObjectDescription.type).\
filter( ObjectInstance.id.in_( any_obj_types_ids(ObjectInstance, req.types) ), ObjectInstance.relative_description_id == ObjectDescription.id, ObjectInstance.frame_id == FrameNode.id ).all()
for id, name, alias, frame, type in objects:
obj = ROSObjectInstance()
obj.id = id
obj.name = name
obj.alias = alias
obj.pose = frame.toROSPoseStamped()
obj.description.type = type
res.objects.append(obj)
return res
def create_object_instance():
pose = ROSPoseStamped()
pose.header.frame_id = "world"
pose.pose.position.x = 0.0
pose.pose.position.y = 0.0
pose.pose.position.z = 0.0
pose.pose.orientation.x = 0.0
pose.pose.orientation.y = 0.0
pose.pose.orientation.z = 0.0
pose.pose.orientation.w = 1.0
inst_ros = ROSObjectInstance()
inst_ros.alias = "mr_objecto"
inst_ros.pose = pose
inst_ros.description = create_object_description()
return inst_ros
def createObjectCb(self, pose):
print 'Create Object from RViz pose'
app = QApplication(sys.argv)
widget = ChooseObjectDescriptionWidget(0)
widget.show()
app.exec_()
desc_name, desc_id = widget.getChoice()
del app, widget
if(desc_id == -1):
new_desc = ROSObjectDescription()
new_desc.type = desc_name
res = call_add_object_descriptions( [ new_desc ] )
print res
desc_id = res.ids[0]
obj = ROSObjectInstance()
obj.description.id = desc_id
obj.pose = pose
res = call_add_object_instances( [obj] )
call_activate_objects( res.ids )
def get_object_instances( req ):
rospy.loginfo( "SEMAP DB SRVs: get_object_instances" )
then = rospy.Time.now()
res = GetObjectInstancesResponse()
if True:
inst = aliased(ObjectInstance)
desc = aliased(ObjectDescription)
frame = aliased(FrameNode)
descs = db().query( desc.id, desc.type, desc ).filter( inst.id.in_( req.ids ), inst.relative_description_id == desc.id ).distinct().all()
descs_time = (rospy.Time.now() - then ).to_sec()
#print descs
#for id, type, desc in descs:
#print id, type
resultset ={}
for id, type, desc in descs:
resultset[id] = desc.toROS()
#print resultset
ros_time = (rospy.Time.now() - then ).to_sec() - descs_time
insts = db().query( inst.id, inst.name, inst.alias, inst.relative_description_id, frame ).filter( inst.id.in_( req.ids ), inst.frame_id == frame.id ).all()
insts_time = (rospy.Time.now() - then ).to_sec() - descs_time - ros_time
for id, name, alias, desc_id, frame in insts:
#print id, alias, desc_id, frame
ros = ROSObjectInstance()
ros.id = id
ros.name = name
if alias:
ros.alias = alias
else:
ros.alias = ""
if frame:
ros.pose = frame.toROSPoseStamped()
else:
ros.pose = ROSPoseStamped()
if desc_id:
ros.description = resultset[desc_id]#.toROS()
else:
ros.description = ROSObjectDescription()
if True:#False:
abs_desc = db().query( ObjectDescription ).filter( ObjectInstance.id == id, ObjectInstance.absolute_description_id == ObjectDescription.id ).scalar()
ros.absolute = abs_desc.toROS()
else:
ros.absolute = ROSObjectDescription()
res.objects.append( ros )
packaging_time = (rospy.Time.now() - then ).to_sec() - descs_time - ros_time- insts_time
rospy.loginfo( "# Descriptions : %d" % len(descs) )
rospy.loginfo( "# Instances : %d" % len(insts) )
rospy.loginfo( " Descriptions : %fs" % descs_time )
rospy.loginfo( " ROS Conversion: %fs" % ros_time )
rospy.loginfo( " Instances : %fs" % insts_time)
rospy.loginfo( " Packaging : %fs" % packaging_time )
rospy.loginfo( "Total %fs" % ( rospy.Time.now() - then ).to_sec() )
then2 = rospy.Time.now()
if False:
rospy.loginfo( "SEMAP DB SRVs: get_object_instances NAIVE" )
objects = db().query( ObjectInstance ).filter( ObjectInstance.id.in_( req.ids ) ).all()
obj_time = (rospy.Time.now() - then2 ).to_sec()
for obj in objects:
ros = obj.toROS()
res.objects.append( ros )
packaging2_time = (rospy.Time.now() - then2 ).to_sec() - obj_time
rospy.loginfo( " Objects : %fs" % obj_time )
rospy.loginfo( " Packaging : %fs" % packaging2_time )
rospy.loginfo( "Total %fs" % ( rospy.Time.now() - then2 ).to_sec() )
return res
def toROS(self):
ros = ROSObjectInstance()
ros.id = self.id
ros.name = str(self.name)
if self.alias:
ros.alias = str(self.alias)
else:
ros.alias = ""
if self.frame:
ros.pose = self.frame.toROSPoseStamped()
else:
ros.pose = ROSPoseStamped()
if self.relative_description:
ros.description = self.relative_description.toROS()
else:
ros.description = ROSObjectDescription()
if self.absolute_description:
ros.absolute = self.absolute_description.toROS()
else:
ros.absolute = ROSObjectDescription()
return ros
def get_object_instances(req):
rospy.loginfo("SEMAP DB SRVs: get_object_instances")
then = rospy.Time.now()
res = GetObjectInstancesResponse()
if True:
inst = aliased(ObjectInstance)
desc = aliased(ObjectDescription)
frame = aliased(FrameNode)
descs = db().query(desc.id, desc.type, desc).filter(
inst.id.in_(req.ids),
inst.relative_description_id == desc.id).distinct().all()
descs_time = (rospy.Time.now() - then).to_sec()
#print descs
#for id, type, desc in descs:
#print id, type
resultset = {}
for id, type, desc in descs:
resultset[id] = desc.toROS()
#print resultset
ros_time = (rospy.Time.now() - then).to_sec() - descs_time
insts = db().query(inst.id, inst.name, inst.alias,
inst.relative_description_id,
frame).filter(inst.id.in_(req.ids),
inst.frame_id == frame.id).all()
insts_time = (rospy.Time.now() - then).to_sec() - descs_time - ros_time
for id, name, alias, desc_id, frame in insts:
#print id, alias, desc_id, frame
ros = ROSObjectInstance()
ros.id = id
ros.name = name
if alias:
ros.alias = alias
else:
ros.alias = ""
if frame:
ros.pose = frame.toROSPoseStamped()
else:
ros.pose = ROSPoseStamped()
if desc_id:
ros.description = resultset[desc_id] #.toROS()
else:
ros.description = ROSObjectDescription()
if True: #False:
abs_desc = db().query(ObjectDescription).filter(
ObjectInstance.id == id,
ObjectInstance.absolute_description_id ==
ObjectDescription.id).scalar()
ros.absolute = abs_desc.toROS()
else:
ros.absolute = ROSObjectDescription()
res.objects.append(ros)
packaging_time = (rospy.Time.now() -
then).to_sec() - descs_time - ros_time - insts_time
rospy.loginfo("# Descriptions : %d" % len(descs))
rospy.loginfo("# Instances : %d" % len(insts))
rospy.loginfo(" Descriptions : %fs" % descs_time)
rospy.loginfo(" ROS Conversion: %fs" % ros_time)
rospy.loginfo(" Instances : %fs" % insts_time)
rospy.loginfo(" Packaging : %fs" % packaging_time)
rospy.loginfo("Total %fs" %
(rospy.Time.now() - then).to_sec())
then2 = rospy.Time.now()
if False:
rospy.loginfo("SEMAP DB SRVs: get_object_instances NAIVE")
objects = db().query(ObjectInstance).filter(
ObjectInstance.id.in_(req.ids)).all()
obj_time = (rospy.Time.now() - then2).to_sec()
for obj in objects:
ros = obj.toROS()
res.objects.append(ros)
packaging2_time = (rospy.Time.now() - then2).to_sec() - obj_time
rospy.loginfo(" Objects : %fs" % obj_time)
rospy.loginfo(" Packaging : %fs" % packaging2_time)
rospy.loginfo("Total %fs" %
(rospy.Time.now() - then2).to_sec())
return res
def toROS( self ):
ros = ROSObjectInstance()
ros.id = self.id
ros.name = str(self.name)
if self.alias:
ros.alias = str(self.alias)
else:
ros.alias = ""
if self.frame:
ros.pose = self.frame.toROSPoseStamped()
else:
ros.pose = ROSPoseStamped()
if self.relative_description:
ros.description = self.relative_description.toROS()
else:
ros.description = ROSObjectDescription()
if self.absolute_description:
ros.absolute = self.absolute_description.toROS()
else:
ros.absolute = ROSObjectDescription()
return ros