コード例 #1
0
def get_containment_pairs( req ):
  rospy.loginfo( "SEMAP DB SRVs: get_containment_pairs" )
  res = GetObjectLocation()

  obj1 = aliased( ObjectInstance )
  geo1 = aliased( GeometryModel )

  obj2 = aliased( ObjectInstance )
  geo2 = aliased( GeometryModel )

  if req.reference_object_types:
    obj1_ids = any_obj_types_ids(obj1, req.refrence_object_types)
  else:
    obj1_ids = any_obj_ids(obj1)

  if req.target_object_types:
    obj2_ids = any_obj_types_ids(obj2, req.target_object_types)
  else:
    obj2_ids = any_obj_ids(obj2)

  ids = db().query( obj1.id, obj2.id ).\
          filter(obj1.id.in_( obj1_ids ), obj1.absolute_description_id == geo1.abstraction_desc, geo1.type == "BoundingBox", \
                 obj2.id.in_( obj2_ids ), obj2.absolute_description_id == geo2.abstraction_desc, geo2.type == "BoundingBox", \
                 SFCGAL_Contains3D( geo1.geometry, geo2.geometry)
                ).all()

  for i1, i2 in ids:
    pair = ObjectPair()
    pair.reference_id = i1
    pair.relations.append("contains")
    pair.target_id = i2

  res.target_object_ids = [id for id, in ids]

  return res
コード例 #2
0
def get_objects_within_volume( req ):
  rospy.loginfo( "SEMAP DB SRVs: get_objects_within_area" )
  res = GetObjectsWithinVolumeResponse()

  tar_obj = aliased( ObjectInstance )
  tar_geo = aliased( GeometryModel )

  if req.target_object_types:
    tar_ids = any_obj_types_ids(tar_obj, req.target_object_types)
  else:
    tar_ids = any_obj_ids(tar_obj)

  if req.fully_within:
    operator = SFCGAL_Contains3D(fromPolygonMesh3D(req.reference_mesh), tar_geo.geometry)
  else:
    operator = or_( SFCGAL_Contains3D(fromPolygonMesh3D(req.reference_mesh), tar_geo.geometry),
                    SFCGAL_Intersects3D(fromPolygonMesh3D(req.reference_mesh), tar_geo.geometry) )

  results = db().query( tar_obj.id ).\
                 filter( tar_obj.id.in_( tar_ids ),
                         tar_obj.absolute_description_id == tar_geo.abstraction_desc,
                         tar_geo.type == req.target_object_geometry_type,
                         operator).all()

  for i in results:
    pair = ObjectPair()
    pair.reference_id = -1
    pair.target_id = i[0]
    pair.relations.append("contained-in-volume")
    res.pairs.append(pair)

  return res
  '''
コード例 #3
0
def get_containment_pairs(req):
    rospy.loginfo("SEMAP DB SRVs: get_containment_pairs")
    res = GetObjectLocation()

    obj1 = aliased(ObjectInstance)
    geo1 = aliased(GeometryModel)

    obj2 = aliased(ObjectInstance)
    geo2 = aliased(GeometryModel)

    if req.reference_object_types:
        obj1_ids = any_obj_types_ids(obj1, req.refrence_object_types)
    else:
        obj1_ids = any_obj_ids(obj1)

    if req.target_object_types:
        obj2_ids = any_obj_types_ids(obj2, req.target_object_types)
    else:
        obj2_ids = any_obj_ids(obj2)

    ids = db().query( obj1.id, obj2.id ).\
            filter(obj1.id.in_( obj1_ids ), obj1.absolute_description_id == geo1.abstraction_desc, geo1.type == "BoundingBox", \
                   obj2.id.in_( obj2_ids ), obj2.absolute_description_id == geo2.abstraction_desc, geo2.type == "BoundingBox", \
                   SFCGAL_Contains3D( geo1.geometry, geo2.geometry)
                  ).all()

    for i1, i2 in ids:
        pair = ObjectPair()
        pair.reference_id = i1
        pair.relations.append("contains")
        pair.target_id = i2

    res.target_object_ids = [id for id, in ids]

    return res
コード例 #4
0
def get_objects_within_area(req):
    rospy.loginfo("SEMAP DB SRVs: get_objects_within_area")
    res = GetObjectsWithinAreaResponse()

    tar_obj = aliased(ObjectInstance)
    tar_geo = aliased(GeometryModel)

    if req.target_object_types:
        tar_ids = any_obj_types_ids(tar_obj, req.target_object_types)
    else:
        tar_ids = any_obj_ids(tar_obj)

    if req.fully_within:
        operator = ST_Within(tar_geo.geometry,
                             fromPolygon2D(req.reference_polygon))
    else:
        operator = or_(
            ST_Overlaps(fromPolygon2D(req.reference_polygon),
                        tar_geo.geometry),
            ST_Within(tar_geo.geometry, fromPolygon2D(req.reference_polygon)))

    results = db().query( tar_obj.id ).\
                   filter( tar_obj.id.in_( tar_ids ),
                           tar_obj.absolute_description_id == tar_geo.abstraction_desc,
                           tar_geo.type == req.target_object_geometry_type,
                           operator).all()

    for i in results:
        pair = ObjectPair()
        pair.reference_id = -1
        pair.target_id = i[0]
        pair.relations.append("contained-in-area")
        res.pairs.append(pair)
    return res
コード例 #5
0
def get_objects_within_range( req ):
  rospy.loginfo( "SEMAP DB SRVs: get_objects_within_range" )
  res = GetObjectsWithinRangeResponse()

  tar_obj = aliased( ObjectInstance )
  tar_geo = aliased( GeometryModel )

  if req.target_object_types:
    tar_ids = any_obj_types_ids(tar_obj, req.target_object_types)
  else:
    tar_ids = any_obj_ids(tar_obj)

  if req.fully_within:
    operator = ST_3DDFullyWithin(fromPoint3D(req.reference_point), tar_geo.geometry, req.distance)
  else:
    operator = ST_3DDWithin(fromPoint3D(req.reference_point), tar_geo.geometry, req.distance)

  results = db().query( tar_obj.id, ST_3DDistance( fromPoint3D(req.reference_point), tar_geo.geometry), ST_3DMaxDistance( fromPoint3D(req.reference_point), tar_geo.geometry), ST_3DClosestPoint(tar_geo.geometry, fromPoint3D(req.reference_point)) ).\
                  filter( tar_obj.id.in_( tar_ids ), tar_obj.absolute_description_id == tar_geo.abstraction_desc, tar_geo.type == req.target_object_geometry_type, operator).\
                  order_by( ST_3DDistance( fromPoint3D(req.reference_point), tar_geo.geometry) ).all()

  for i, min, max, point in results:
    print i, min, max, point
    pair = ObjectPair()
    pair.reference_id = -1
    pair.target_id = i
    pair.max_dist = max
    pair.max_dist_line[0] = req.reference_point
    pair.max_dist_line[1] = toPoint3D( point )
    pair.min_dist = min
    pair.min_dist_line[0] = req.reference_point
    pair.min_dist_line[1] = toPoint3D( point )
    res.pairs.append(pair)

  return res
コード例 #6
0
def get_objects_within_range(req):
    rospy.loginfo("SEMAP DB SRVs: get_objects_within_range")
    res = GetObjectsWithinRangeResponse()

    tar_obj = aliased(ObjectInstance)
    tar_geo = aliased(GeometryModel)

    if req.target_object_types:
        tar_ids = any_obj_types_ids(tar_obj, req.target_object_types)
    else:
        tar_ids = any_obj_ids(tar_obj)

    if req.fully_within:
        operator = ST_3DDFullyWithin(fromPoint3D(req.reference_point),
                                     tar_geo.geometry, req.distance)
    else:
        operator = ST_3DDWithin(fromPoint3D(req.reference_point),
                                tar_geo.geometry, req.distance)

    results = db().query( tar_obj.id, ST_3DDistance( fromPoint3D(req.reference_point), tar_geo.geometry), ST_3DMaxDistance( fromPoint3D(req.reference_point), tar_geo.geometry), ST_3DClosestPoint(tar_geo.geometry, fromPoint3D(req.reference_point)) ).\
                    filter( tar_obj.id.in_( tar_ids ), tar_obj.absolute_description_id == tar_geo.abstraction_desc, tar_geo.type == req.target_object_geometry_type, operator).\
                    order_by( ST_3DDistance( fromPoint3D(req.reference_point), tar_geo.geometry) ).all()

    for i, min, max, point in results:
        print i, min, max, point
        pair = ObjectPair()
        pair.reference_id = -1
        pair.target_id = i
        pair.max_dist = max
        pair.max_dist_line[0] = req.reference_point
        pair.max_dist_line[1] = toPoint3D(point)
        pair.min_dist = min
        pair.min_dist_line[0] = req.reference_point
        pair.min_dist_line[1] = toPoint3D(point)
        res.pairs.append(pair)

    return res
コード例 #7
0
def get_distance_between_objects( req ):
  rospy.loginfo( "SEMAP DB SRVs: get_distance_between_objects3d" )
  res = GetDistanceBetweenObjectsResponse()

  ref_obj = aliased( ObjectInstance )
  ref_geo = aliased( GeometryModel )

  tar_obj = aliased( ObjectInstance )
  tar_geo = aliased( GeometryModel )

  if req.reference_object_geometry_type not in ["Position3D", "AxisAligned3D", "BoundingBox",  "BoundingHull", "Body"]:
    rospy.logerr("SEMAP DB SRVs: get_distance_between_objects3d was called with %s which is not a valid 2D geometry type" % req.reference_object_geometry_type)
  else:
    if req.reference_object_types:
      ref_ids = any_obj_types_ids(ref_obj, req.reference_object_types)
    else:
      ref_ids = any_obj_ids(ref_obj)

    if req.target_object_types:
      tar_ids = any_obj_types_ids(tar_obj, req.target_object_types)
    else:
      tar_ids = any_obj_ids(tar_obj)

    if req.max_distance:
      distance = ST_3DMaxDistance(ref_geo.geometry, tar_geo.geometry)
      line = ST_3DLongestLine(ref_geo.geometry, tar_geo.geometry)
    else:
      distance = ST_3DDistance(ref_geo.geometry, tar_geo.geometry)
      line = ST_3DShortestLine(ref_geo.geometry, tar_geo.geometry)

    if req.return_points:
      query = db().query( ref_obj.id, tar_obj.id, distance, line).\
                   filter( ref_obj.id.in_( ref_ids ), tar_obj.id.in_( tar_ids ), \
                           ref_obj.absolute_description_id == ref_geo.abstraction_desc,
                           tar_obj.absolute_description_id == tar_geo.abstraction_desc,
                           ref_geo.type == req.reference_object_geometry_type,
                           tar_geo.type == req.target_object_geometry_type )
    else:
      query = db().query( ref_obj.id, tar_obj.id, distance).\
           filter( ref_obj.id.in_( ref_ids ), tar_obj.id.in_( tar_ids ), \
                   ref_obj.absolute_description_id == ref_geo.abstraction_desc,
                   tar_obj.absolute_description_id == tar_geo.abstraction_desc,
                   ref_geo.type == req.reference_object_geometry_type,
                   tar_geo.type == req.target_object_geometry_type )

    if req.min_range:
      query = query.filter( distance > req.min_range )

    if req.min_range:
      query = query.filter( distance < req.max_range )

    if req.sort_descending:
      query = query.order_by( desc(distance) )
    else:
      query = query.order_by( distance )

    results = query.all()

    if req.return_points:
      for i, j, dist, points in results:
        pair = ObjectPair()
        pair.reference_id = i
        pair.target_id = j
        if req.max_distance:
          pair.max_dist = dist
          pair.max_dist_line[0] = toPoint3D( db().execute( ST_PointN( points, 1 ) ).scalar() )
          pair.max_dist_line[1] = toPoint3D( db().execute( ST_PointN( points, 2 ) ).scalar() )
        else:
          pair.min_dist = dist
          pair.min_dist_line[0] = toPoint3D( db().execute( ST_PointN( points, 1 ) ).scalar() )
          pair.min_dist_line[1] = toPoint3D( db().execute( ST_PointN( points, 2 ) ).scalar() )
        res.pairs.append(pair)
    else:
      for i, j, dist in results:
        pair = ObjectPair()
        pair.reference_id = i
        pair.target_id = j
        if req.max_distance:
          pair.max_dist = dist
        else:
          pair.min_dist = dist
        res.pairs.append(pair)

  return res
コード例 #8
0
def get_objects_intersect_objects( req ):
  then = rospy.Time.now()
  rospy.logdebug( "SEMAP DB SRVs: get_objects_intersect_objects" )
  res = GetObjectsIntersectObjectsResponse()

  if req.reference.geometry_type in [ "AxisAligned3D", "BoundingBox", "Body"  ]: #"Position3D",  "BoundingHull"
    ref_status = "3D"
  elif req.reference.geometry_type in [ "Position2D", "FootprintBox", "FootprintHull" ]:
    ref_status = "2D"
  else:
    rospy.logerr("SEMAP DB SRVs: get_objects_in_objects was called with %s as reference geometry thats currently not supported",  req.reference.geometry_type)
    return res

  if req.target.geometry_type in [ "AxisAligned3D", "BoundingBox", "Body" ]: #"Position3D",  "BoundingHull"
    tar_status = "3D"
  elif req.target.geometry_type in [ "Position2D", "FootprintBox", "FootprintHull" ]:
    tar_status = "2D"
  else:
    rospy.logerr("SEMAP DB SRVs: get_objects_in_objects was called with %s as target geometry thats currently not supported",  req.target.geometry_type)
    return res

  if ref_status != tar_status:
    rospy.logerr("SEMAP DB SRVs: get_objects_in_objects was called with requests of inconsistent dimensionality")
    return res

  ref_obj = aliased( ObjectInstance )
  ref_geo = aliased( GeometryModel )

  tar_obj = aliased( ObjectInstance )
  tar_geo = aliased( GeometryModel )

  #if req.reference.ids and req.reference.ids != (0,):
  #  ref_ids = req.reference.ids
  #elif req.reference.class_types:
  #  ref_ids = any_obj_types_ids( ref_obj, req.reference.class_types ).all()
  #else:
  #  ref_ids = any_obj_ids( ref_obj ).all()
  #if req.target.ids and req.target.ids != (0,):
  #  tar_ids = req.target.ids
  #elif req.target.class_types:
  #  tar_ids = any_obj_types_ids( tar_obj, req.target.class_types ).all()
  #else:
  #  tar_ids = any_obj_ids( tar_obj ).all()


  ref_ids =constrain_obj_ids(ref_obj, req.reference).all()
  tar_ids =constrain_obj_ids(tar_obj, req.target).all()

  if ref_status == "2D":
    operator = ST_Overlaps(tar_geo.geometry, ref_geo.geometry)
  else:
    operator = SFCGAL_Intersects3D(ref_geo.geometry, tar_geo.geometry)

  retrieval =  ( rospy.Time.now() - then ).to_sec()

  if req.reference.geometry_type == "Body" and req.target.geometry_type == "Body":
    ids = db().query( ref_obj.id, tar_obj.id ).\
               filter( ref_obj.id.in_( ref_ids ),
                       ref_obj.absolute_description_id == ref_geo.geometry_desc,
                       ref_geo.type == req.reference.geometry_type,
                       tar_obj.id.in_( tar_ids ),
                       tar_obj.absolute_description_id == tar_geo.geometry_desc,
                       tar_geo.type == req.target.geometry_type,
                       ref_obj.id != tar_obj.id,
                       operator).order_by(ref_obj.id).all()
  else:
    #print  'ref sql', constrain_obj_ids(ref_obj, req.reference)
    #print  'ref geo', constrain_obj_ids(ref_obj, req.reference).all()
    res.refs =  len(ref_ids)
    res.tars =  len(tar_ids)
    res.num_tests =  len(ref_ids) * len(tar_ids)
    #test = db().query( ref_obj.id, tar_obj.id ).\
      #filter(
              #ref_geo.id.in_( constrain_geo_ids(ref_obj, ref_geo, req.reference).correlate(ref_obj.absolute_description_id == ref_geo.geometry_desc) ),
              #tar_geo.id.in_( constrain_geo_ids(tar_obj, tar_geo, req.target).correlate(tar_obj.absolute_description_id == tar_geo.geometry_desc) ),
              #ref_obj.id != tar_obj.id,
              #operator
            #).all()

    #print  'ref', constrain_obj_ids(ref_obj, req.reference).all()

    #print  'tar', constrain_obj_ids(tar_obj, req.target).all()

    #print test

    ids = db().query( ref_obj.id, tar_obj.id ).\
      filter(  #or_( ref_obj.id.in_( ref_ids ), ref_obj.id.in_( req.reference.ids ))
                   ref_obj.id.in_( constrain_obj_ids(ref_obj, req.reference) ),
                   ref_obj.absolute_description_id == ref_geo.abstraction_desc,
                   ref_geo.type == req.reference.geometry_type,

                   tar_obj.id.in_( constrain_obj_ids(tar_obj, req.target) ),
                   tar_obj.absolute_description_id == tar_geo.abstraction_desc,
                   tar_geo.type == req.target.geometry_type,
                   ref_obj.id != tar_obj.id,
                   operator).order_by(ref_obj.id).all()

  testing = ( rospy.Time.now() - then ).to_sec() - retrieval

  if ids:
    old_i = None
    one_to_many = OneToMany()
    one_to_many.reference_id = ids[0][0]
    for i, d in ids:
      if one_to_many.reference_id != i:
        res.results.append(one_to_many)
        one_to_many = OneToMany()
        one_to_many.reference_id = i
      one_to_many.target_ids.append(d)
      pair = ObjectPair()
      pair.reference_id = i
      pair.target_id = d
      pair.relations.append("intersects")
      res.pairs.append(pair)
    res.results.append(one_to_many)
  packaging =  ( rospy.Time.now() - then ).to_sec() - retrieval - testing
  #rospy.loginfo( "ObjectsIntersectObjects %f:" % ( rospy.Time.now() - then ).to_sec() )
  #rospy.loginfo( "  Retrieving     %f:" % retrieval)
  #rospy.loginfo( "  Testing        %f:" % testing)
  #rospy.loginfo( "  Packaging      %f:" % packaging )

  #print '#pairs', len(res.pairs)

  return res
コード例 #9
0
def get_objects_on_objects( req ):

  rospy.logdebug( "SEMAP DB SRVs: get_objects_on_objects" )
  res = GetObjectsOnObjectsResponse()

  if req.reference.geometry_type not in [ "BoundingBox" ]: #"Position3D",  "BoundingHull"
    rospy.logerr("SEMAP DB SRVs: get_objects_in_objects was called with %s as reference geometry thats currently not supported",  req.reference.geometry_type)
    return res

  if req.target.geometry_type not in [ "BoundingBox" ]: #"Position3D",  "BoundingHull"
    rospy.logerr("SEMAP DB SRVs: get_objects_in_objects was called with %s as target geometry thats currently not supported",  req.reference.geometry_type)
    return res

  ref_obj = aliased( ObjectInstance )
  ref_geo = aliased( GeometryModel )
  ref_top = aliased( GeometryModel )

  tar_obj = aliased( ObjectInstance )
  tar_geo = aliased( GeometryModel )

  #if req.reference.ids and req.reference.ids != (0,):
    #ref_ids = req.reference.ids
  #elif req.reference.class_types:
    #ref_ids = any_obj_types_ids( ref_obj, req.reference.class_types ).all()
  #else:
    #ref_ids = any_obj_ids( ref_obj ).all()

  #if req.target.ids and req.target.ids != (0,):
    #tar_ids = req.target.ids
  #elif req.target.class_types:
    #tar_ids = any_obj_types_ids( tar_obj, req.target.class_types ).all()
  #else:
    #tar_ids = any_obj_ids( tar_obj ).all()


  ref_ids =constrain_obj_ids(ref_obj, req.reference).all()
  tar_ids =constrain_obj_ids(tar_obj, req.target).all()
  res.refs =  len(ref_ids)
  res.tars =  len(tar_ids)
  res.num_tests =  len(ref_ids) * len(tar_ids)

  print 'ON TESTS', res.num_tests
  print 'refs', ref_ids
  print 'refs', tar_ids

  pairs = db().query( ref_obj.id, tar_obj.id ).\
              filter( ref_obj.id.in_( ref_ids ),
                      ref_obj.absolute_description_id == ref_geo.abstraction_desc,
                      ref_geo.type == req.reference.geometry_type,
                      ref_obj.absolute_description_id == ref_top.abstraction_desc,
                      ref_top.type == "TopProjection",
                      tar_obj.id.in_( tar_ids ),
                      tar_obj.absolute_description_id == tar_geo.abstraction_desc, tar_geo.type == req.target.geometry_type,
                      SFCGAL_Contains3D( ref_top.geometry, tar_geo.geometry),
                      SFCGAL_Distance3D( ref_geo.geometry, tar_geo.geometry) < req.threshold).order_by(ref_obj.id).all()

  if pairs:
    old_ref = None
    one_to_many = OneToMany()
    one_to_many.reference_id = pairs[0][0]
    for ref_id, tar_id in pairs:
      if one_to_many.reference_id != ref_id:
        res.results.append(one_to_many)
        one_to_many = OneToMany()
        one_to_many.reference_id = ref_id
      one_to_many.target_ids.append(tar_id)
      pair = ObjectPair()
      pair.reference_id = ref_id
      pair.target_id = tar_id
      pair.relations.append("contains")
      res.pairs.append(pair)
    res.results.append(one_to_many)

  return res
コード例 #10
0
def get_distance_between_objects(req):
    rospy.loginfo("SEMAP DB SRVs: get_distance_between_objects3d")
    res = GetDistanceBetweenObjectsResponse()

    ref_obj = aliased(ObjectInstance)
    ref_geo = aliased(GeometryModel)

    tar_obj = aliased(ObjectInstance)
    tar_geo = aliased(GeometryModel)

    if req.reference_object_geometry_type not in [
            "Position3D", "AxisAligned3D", "BoundingBox", "BoundingHull",
            "Body"
    ]:
        rospy.logerr(
            "SEMAP DB SRVs: get_distance_between_objects3d was called with %s which is not a valid 2D geometry type"
            % req.reference_object_geometry_type)
    else:
        if req.reference_object_types:
            ref_ids = any_obj_types_ids(ref_obj, req.reference_object_types)
        else:
            ref_ids = any_obj_ids(ref_obj)

        if req.target_object_types:
            tar_ids = any_obj_types_ids(tar_obj, req.target_object_types)
        else:
            tar_ids = any_obj_ids(tar_obj)

        if req.max_distance:
            distance = ST_3DMaxDistance(ref_geo.geometry, tar_geo.geometry)
            line = ST_3DLongestLine(ref_geo.geometry, tar_geo.geometry)
        else:
            distance = ST_3DDistance(ref_geo.geometry, tar_geo.geometry)
            line = ST_3DShortestLine(ref_geo.geometry, tar_geo.geometry)

        if req.return_points:
            query = db().query( ref_obj.id, tar_obj.id, distance, line).\
                         filter( ref_obj.id.in_( ref_ids ), tar_obj.id.in_( tar_ids ), \
                                 ref_obj.absolute_description_id == ref_geo.abstraction_desc,
                                 tar_obj.absolute_description_id == tar_geo.abstraction_desc,
                                 ref_geo.type == req.reference_object_geometry_type,
                                 tar_geo.type == req.target_object_geometry_type )
        else:
            query = db().query( ref_obj.id, tar_obj.id, distance).\
                 filter( ref_obj.id.in_( ref_ids ), tar_obj.id.in_( tar_ids ), \
                         ref_obj.absolute_description_id == ref_geo.abstraction_desc,
                         tar_obj.absolute_description_id == tar_geo.abstraction_desc,
                         ref_geo.type == req.reference_object_geometry_type,
                         tar_geo.type == req.target_object_geometry_type )

        if req.min_range:
            query = query.filter(distance > req.min_range)

        if req.min_range:
            query = query.filter(distance < req.max_range)

        if req.sort_descending:
            query = query.order_by(desc(distance))
        else:
            query = query.order_by(distance)

        results = query.all()

        if req.return_points:
            for i, j, dist, points in results:
                pair = ObjectPair()
                pair.reference_id = i
                pair.target_id = j
                if req.max_distance:
                    pair.max_dist = dist
                    pair.max_dist_line[0] = toPoint3D(db().execute(
                        ST_PointN(points, 1)).scalar())
                    pair.max_dist_line[1] = toPoint3D(db().execute(
                        ST_PointN(points, 2)).scalar())
                else:
                    pair.min_dist = dist
                    pair.min_dist_line[0] = toPoint3D(db().execute(
                        ST_PointN(points, 1)).scalar())
                    pair.min_dist_line[1] = toPoint3D(db().execute(
                        ST_PointN(points, 2)).scalar())
                res.pairs.append(pair)
        else:
            for i, j, dist in results:
                pair = ObjectPair()
                pair.reference_id = i
                pair.target_id = j
                if req.max_distance:
                    pair.max_dist = dist
                else:
                    pair.min_dist = dist
                res.pairs.append(pair)

    return res
コード例 #11
0
def get_objects_intersect_objects(req):
    then = rospy.Time.now()
    rospy.logdebug("SEMAP DB SRVs: get_objects_intersect_objects")
    res = GetObjectsIntersectObjectsResponse()

    if req.reference.geometry_type in ["AxisAligned3D", "BoundingBox",
                                       "Body"]:  #"Position3D",  "BoundingHull"
        ref_status = "3D"
    elif req.reference.geometry_type in [
            "Position2D", "FootprintBox", "FootprintHull"
    ]:
        ref_status = "2D"
    else:
        rospy.logerr(
            "SEMAP DB SRVs: get_objects_in_objects was called with %s as reference geometry thats currently not supported",
            req.reference.geometry_type)
        return res

    if req.target.geometry_type in ["AxisAligned3D", "BoundingBox",
                                    "Body"]:  #"Position3D",  "BoundingHull"
        tar_status = "3D"
    elif req.target.geometry_type in [
            "Position2D", "FootprintBox", "FootprintHull"
    ]:
        tar_status = "2D"
    else:
        rospy.logerr(
            "SEMAP DB SRVs: get_objects_in_objects was called with %s as target geometry thats currently not supported",
            req.target.geometry_type)
        return res

    if ref_status != tar_status:
        rospy.logerr(
            "SEMAP DB SRVs: get_objects_in_objects was called with requests of inconsistent dimensionality"
        )
        return res

    ref_obj = aliased(ObjectInstance)
    ref_geo = aliased(GeometryModel)

    tar_obj = aliased(ObjectInstance)
    tar_geo = aliased(GeometryModel)

    #if req.reference.ids and req.reference.ids != (0,):
    #  ref_ids = req.reference.ids
    #elif req.reference.class_types:
    #  ref_ids = any_obj_types_ids( ref_obj, req.reference.class_types ).all()
    #else:
    #  ref_ids = any_obj_ids( ref_obj ).all()
    #if req.target.ids and req.target.ids != (0,):
    #  tar_ids = req.target.ids
    #elif req.target.class_types:
    #  tar_ids = any_obj_types_ids( tar_obj, req.target.class_types ).all()
    #else:
    #  tar_ids = any_obj_ids( tar_obj ).all()

    ref_ids = constrain_obj_ids(ref_obj, req.reference).all()
    tar_ids = constrain_obj_ids(tar_obj, req.target).all()

    if ref_status == "2D":
        operator = ST_Overlaps(tar_geo.geometry, ref_geo.geometry)
    else:
        operator = SFCGAL_Intersects3D(ref_geo.geometry, tar_geo.geometry)

    retrieval = (rospy.Time.now() - then).to_sec()

    if req.reference.geometry_type == "Body" and req.target.geometry_type == "Body":
        ids = db().query( ref_obj.id, tar_obj.id ).\
                   filter( ref_obj.id.in_( ref_ids ),
                           ref_obj.absolute_description_id == ref_geo.geometry_desc,
                           ref_geo.type == req.reference.geometry_type,
                           tar_obj.id.in_( tar_ids ),
                           tar_obj.absolute_description_id == tar_geo.geometry_desc,
                           tar_geo.type == req.target.geometry_type,
                           ref_obj.id != tar_obj.id,
                           operator).order_by(ref_obj.id).all()
    else:
        #print  'ref sql', constrain_obj_ids(ref_obj, req.reference)
        #print  'ref geo', constrain_obj_ids(ref_obj, req.reference).all()
        res.refs = len(ref_ids)
        res.tars = len(tar_ids)
        res.num_tests = len(ref_ids) * len(tar_ids)
        #test = db().query( ref_obj.id, tar_obj.id ).\
        #filter(
        #ref_geo.id.in_( constrain_geo_ids(ref_obj, ref_geo, req.reference).correlate(ref_obj.absolute_description_id == ref_geo.geometry_desc) ),
        #tar_geo.id.in_( constrain_geo_ids(tar_obj, tar_geo, req.target).correlate(tar_obj.absolute_description_id == tar_geo.geometry_desc) ),
        #ref_obj.id != tar_obj.id,
        #operator
        #).all()

        #print  'ref', constrain_obj_ids(ref_obj, req.reference).all()

        #print  'tar', constrain_obj_ids(tar_obj, req.target).all()

        #print test

        ids = db().query( ref_obj.id, tar_obj.id ).\
          filter(  #or_( ref_obj.id.in_( ref_ids ), ref_obj.id.in_( req.reference.ids ))
                       ref_obj.id.in_( constrain_obj_ids(ref_obj, req.reference) ),
                       ref_obj.absolute_description_id == ref_geo.abstraction_desc,
                       ref_geo.type == req.reference.geometry_type,

                       tar_obj.id.in_( constrain_obj_ids(tar_obj, req.target) ),
                       tar_obj.absolute_description_id == tar_geo.abstraction_desc,
                       tar_geo.type == req.target.geometry_type,
                       ref_obj.id != tar_obj.id,
                       operator).order_by(ref_obj.id).all()

    testing = (rospy.Time.now() - then).to_sec() - retrieval

    if ids:
        old_i = None
        one_to_many = OneToMany()
        one_to_many.reference_id = ids[0][0]
        for i, d in ids:
            if one_to_many.reference_id != i:
                res.results.append(one_to_many)
                one_to_many = OneToMany()
                one_to_many.reference_id = i
            one_to_many.target_ids.append(d)
            pair = ObjectPair()
            pair.reference_id = i
            pair.target_id = d
            pair.relations.append("intersects")
            res.pairs.append(pair)
        res.results.append(one_to_many)
    packaging = (rospy.Time.now() - then).to_sec() - retrieval - testing
    #rospy.loginfo( "ObjectsIntersectObjects %f:" % ( rospy.Time.now() - then ).to_sec() )
    #rospy.loginfo( "  Retrieving     %f:" % retrieval)
    #rospy.loginfo( "  Testing        %f:" % testing)
    #rospy.loginfo( "  Packaging      %f:" % packaging )

    #print '#pairs', len(res.pairs)

    return res
コード例 #12
0
def get_objects_on_objects(req):

    rospy.logdebug("SEMAP DB SRVs: get_objects_on_objects")
    res = GetObjectsOnObjectsResponse()

    if req.reference.geometry_type not in ["BoundingBox"
                                           ]:  #"Position3D",  "BoundingHull"
        rospy.logerr(
            "SEMAP DB SRVs: get_objects_in_objects was called with %s as reference geometry thats currently not supported",
            req.reference.geometry_type)
        return res

    if req.target.geometry_type not in ["BoundingBox"
                                        ]:  #"Position3D",  "BoundingHull"
        rospy.logerr(
            "SEMAP DB SRVs: get_objects_in_objects was called with %s as target geometry thats currently not supported",
            req.reference.geometry_type)
        return res

    ref_obj = aliased(ObjectInstance)
    ref_geo = aliased(GeometryModel)
    ref_top = aliased(GeometryModel)

    tar_obj = aliased(ObjectInstance)
    tar_geo = aliased(GeometryModel)

    #if req.reference.ids and req.reference.ids != (0,):
    #ref_ids = req.reference.ids
    #elif req.reference.class_types:
    #ref_ids = any_obj_types_ids( ref_obj, req.reference.class_types ).all()
    #else:
    #ref_ids = any_obj_ids( ref_obj ).all()

    #if req.target.ids and req.target.ids != (0,):
    #tar_ids = req.target.ids
    #elif req.target.class_types:
    #tar_ids = any_obj_types_ids( tar_obj, req.target.class_types ).all()
    #else:
    #tar_ids = any_obj_ids( tar_obj ).all()

    ref_ids = constrain_obj_ids(ref_obj, req.reference).all()
    tar_ids = constrain_obj_ids(tar_obj, req.target).all()
    res.refs = len(ref_ids)
    res.tars = len(tar_ids)
    res.num_tests = len(ref_ids) * len(tar_ids)

    print 'ON TESTS', res.num_tests
    print 'refs', ref_ids
    print 'refs', tar_ids

    pairs = db().query( ref_obj.id, tar_obj.id ).\
                filter( ref_obj.id.in_( ref_ids ),
                        ref_obj.absolute_description_id == ref_geo.abstraction_desc,
                        ref_geo.type == req.reference.geometry_type,
                        ref_obj.absolute_description_id == ref_top.abstraction_desc,
                        ref_top.type == "TopProjection",
                        tar_obj.id.in_( tar_ids ),
                        tar_obj.absolute_description_id == tar_geo.abstraction_desc, tar_geo.type == req.target.geometry_type,
                        SFCGAL_Contains3D( ref_top.geometry, tar_geo.geometry),
                        SFCGAL_Distance3D( ref_geo.geometry, tar_geo.geometry) < req.threshold).order_by(ref_obj.id).all()

    if pairs:
        old_ref = None
        one_to_many = OneToMany()
        one_to_many.reference_id = pairs[0][0]
        for ref_id, tar_id in pairs:
            if one_to_many.reference_id != ref_id:
                res.results.append(one_to_many)
                one_to_many = OneToMany()
                one_to_many.reference_id = ref_id
            one_to_many.target_ids.append(tar_id)
            pair = ObjectPair()
            pair.reference_id = ref_id
            pair.target_id = tar_id
            pair.relations.append("contains")
            res.pairs.append(pair)
        res.results.append(one_to_many)

    return res