Example #1
0
  def toROS( self ):
    ros = ROSObjectDescription()
    ros.id = self.id
    ros.type = str( self.type )

    if self.geometries:
      for model in self.geometries:
        if model.geometry_type == 'POINT2D':
          ros.geometries.point2d_models.append( model.toROSPoint2DModel() )
        elif model.geometry_type == 'POSE2D':
          ros.geometries.pose2d_models.append( model.toROSPose2DModel() )
        elif model.geometry_type == 'POLYGON2D':
          ros.geometries.polygon2d_models.append( model.toROSPolygon2DModel() )
        elif model.geometry_type == 'POINT3D':
          ros.geometries.point3d_models.append( model.toROSPoint3DModel() )
        elif model.geometry_type == 'POSE3D':
          ros.geometries.pose3d_models.append( model.toROSPose3DModel() )
        elif model.geometry_type == 'POLYGON3D':
          ros.geometries.polygon3d_models.append( model.toROSPolygon3DModel() )
        elif model.geometry_type == 'TRIANGLEMESH3D':
          ros.geometries.trianglemesh3d_models.append( model.toROSTriangleMesh3DModel() )
        elif model.geometry_type == 'POLYGONMESH3D':
          ros.geometries.polygonmesh3d_models.append( model.toROSPolygonMesh3DModel() )
        else:
          print 'ERROR: found unknown geometry type:', model.geometry_type

    if self.abstractions:
      for model in self.abstractions:
        if model.geometry_type == 'POINT2D':
          ros.abstractions.point2d_models.append( model.toROSPoint2DModel() )
        elif model.geometry_type == 'POSE2D':
          ros.abstractions.pose2d_models.append( model.toROSPose2DModel() )
        elif model.geometry_type == 'POLYGON2D':
          ros.abstractions.polygon2d_models.append( model.toROSPolygon2DModel() )
        elif model.geometry_type == 'POINT3D':
          ros.abstractions.point3d_models.append( model.toROSPoint3DModel() )
        elif model.geometry_type == 'POSE3D':
          ros.abstractions.pose3d_models.append( model.toROSPose3DModel() )
        elif model.geometry_type == 'POLYGON3D':
          ros.abstractions.polygon3d_models.append( model.toROSPolygon3DModel() )
        elif model.geometry_type == 'TRIANGLEMESH3D':
          ros.abstractions.trianglemesh3d_models.append( model.toROSTriangleMesh3DModel() )
        elif model.geometry_type == 'POLYGONMESH3D':
          ros.abstractions.polygonmesh3d_models.append( model.toROSPolygonMesh3DModel() )
        else:
          print 'ERROR: found unknown geometry type:', model.geometry_type

    return ros
def create_object_description(type, pose, path):

  pose_ = ROSPose()
  pose_.position.x = pose[0][0]
  pose_.position.y = pose[0][1]
  pose_.position.z = pose[0][2]
  pose_.orientation.x = pose[1][0]
  pose_.orientation.y = pose[1][1]
  pose_.orientation.z = pose[1][2]
  pose_.orientation.w = pose[1][3]

  desc_ros = ROSObjectDescription()
  desc_ros.type = type
  mesh = importFromFileToMesh(path)
  model = create_trianglemesh3d_model("Body", pose_, mesh)
  desc_ros.trianglemesh3d_models.append(model)

  return desc_ros
def get_object_descriptions_list( req ):
  rospy.loginfo( "SEMAP DB SRVs: get_object_descriptions_list" )
  res = GetObjectDescriptionsListResponse()
  descriptions =[]
  try:
    ids = desc_ids_by_type(ObjectDescription, req.types).all()
    descriptions = db().query(ObjectDescription.id, ObjectDescription.type).distinct().filter(ObjectDescription.id.in_( ids ), ~ObjectDescription.type.match('absolute_description_')).all()
    for id, type in descriptions:
      desc = ROSObjectDescription()
      desc.id = id
      desc.type = type
      res.descriptions.append(desc)
    rospy.loginfo( "return proper res" )
    return res
  except exc.SQLAlchemyError, e:
      if len(descriptions) == 0:
        rospy.loginfo( "no desc found" )
      rospy.loginfo( "sql error %s" % e)
      rospy.loginfo( "got %d descriptions" % len( descriptions ) )
Example #4
0
def get_object_descriptions_list(req):
    rospy.loginfo("SEMAP DB SRVs: get_object_descriptions_list")
    res = GetObjectDescriptionsListResponse()
    descriptions = []
    try:
        ids = desc_ids_by_type(ObjectDescription, req.types).all()
        descriptions = db().query(
            ObjectDescription.id, ObjectDescription.type).distinct().filter(
                ObjectDescription.id.in_(ids),
                ~ObjectDescription.type.match('absolute_description_')).all()
        for id, type in descriptions:
            desc = ROSObjectDescription()
            desc.id = id
            desc.type = type
            res.descriptions.append(desc)
        rospy.loginfo("return proper res")
        return res
    except exc.SQLAlchemyError, e:
        if len(descriptions) == 0:
            rospy.loginfo("no desc found")
        rospy.loginfo("sql error %s" % e)
        rospy.loginfo("got %d descriptions" % len(descriptions))
Example #5
0
  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 )
Example #6
0
def get_object_descriptions_list(req):
    rospy.loginfo("SpatialDB SRVs: get_object_descriptions_list")
    res = GetObjectDescriptionsListResponse()
    rospy.loginfo("before SpatialDB SRVs: get_object_descriptions_list")
    descriptions =[]
    #import logging
    try:
      descriptions = db().query(ObjectDescription.id, ObjectDescription.type).all()

      for id, type in descriptions:
        desc = ROSObjectDescription()
        desc.id = id
        desc.type = type
        res.descriptions.append(desc)

      rospy.loginfo("return proper res")
      return res

    except exc.SQLAlchemyError, e:
        if len(descriptions) == 0:
          rospy.loginfo("no desc found")
        rospy.loginfo("sql error %s" % e)
        rospy.loginfo('got %d descriptions' % len(descriptions))
Example #7
0
    def toROS(self):
        ros = ROSObjectDescription()
        ros.id = self.id
        ros.type = str(self.type)

        if self.geometries:
            for model in self.geometries:
                if model.geometry_type == 'POINT2D':
                    ros.geometries.point2d_models.append(
                        model.toROSPoint2DModel())
                elif model.geometry_type == 'POSE2D':
                    ros.geometries.pose2d_models.append(
                        model.toROSPose2DModel())
                elif model.geometry_type == 'POLYGON2D':
                    ros.geometries.polygon2d_models.append(
                        model.toROSPolygon2DModel())
                elif model.geometry_type == 'POINT3D':
                    ros.geometries.point3d_models.append(
                        model.toROSPoint3DModel())
                elif model.geometry_type == 'POSE3D':
                    ros.geometries.pose3d_models.append(
                        model.toROSPose3DModel())
                elif model.geometry_type == 'POLYGON3D':
                    ros.geometries.polygon3d_models.append(
                        model.toROSPolygon3DModel())
                elif model.geometry_type == 'TRIANGLEMESH3D':
                    ros.geometries.trianglemesh3d_models.append(
                        model.toROSTriangleMesh3DModel())
                elif model.geometry_type == 'POLYGONMESH3D':
                    ros.geometries.polygonmesh3d_models.append(
                        model.toROSPolygonMesh3DModel())
                else:
                    print 'ERROR: found unknown geometry type:', model.geometry_type

        if self.abstractions:
            for model in self.abstractions:
                if model.geometry_type == 'POINT2D':
                    ros.abstractions.point2d_models.append(
                        model.toROSPoint2DModel())
                elif model.geometry_type == 'POSE2D':
                    ros.abstractions.pose2d_models.append(
                        model.toROSPose2DModel())
                elif model.geometry_type == 'POLYGON2D':
                    ros.abstractions.polygon2d_models.append(
                        model.toROSPolygon2DModel())
                elif model.geometry_type == 'POINT3D':
                    ros.abstractions.point3d_models.append(
                        model.toROSPoint3DModel())
                elif model.geometry_type == 'POSE3D':
                    ros.abstractions.pose3d_models.append(
                        model.toROSPose3DModel())
                elif model.geometry_type == 'POLYGON3D':
                    ros.abstractions.polygon3d_models.append(
                        model.toROSPolygon3DModel())
                elif model.geometry_type == 'TRIANGLEMESH3D':
                    ros.abstractions.trianglemesh3d_models.append(
                        model.toROSTriangleMesh3DModel())
                elif model.geometry_type == 'POLYGONMESH3D':
                    ros.abstractions.polygonmesh3d_models.append(
                        model.toROSPolygonMesh3DModel())
                else:
                    print 'ERROR: found unknown geometry type:', model.geometry_type

        return ros
def create_complete_object_description():
  point2dmodel = Point2DModel()
  point2dmodel.type = '2dpoint'
  point2dmodel.geometry.x = -0.5
  point2dmodel.geometry.y = 0.0
  point2dmodel.geometry.z = 0.0

  pose2dmodel = Pose2DModel()
  pose2dmodel.type = '2dpose'
  pose2dmodel.pose = ROSPose2D()
  pose2dmodel.pose.x = 0.5
  pose2dmodel.pose.y = 1.5
  pose2dmodel.pose.theta = 1.5

  point0 = ROSPoint32(0, 0, 0)
  point1 = ROSPoint32(1, 0, 0)
  point2 = ROSPoint32(1, 1, 0)
  point3 = ROSPoint32(0, 1, 0)
  point4 = ROSPoint32(-1, 0, 0.5)
  point5 = ROSPoint32(-1, 1, 0.5)

  point6 = ROSPoint32(1, 1, 0)
  point7 = ROSPoint32(2, 1, 0)
  point8 = ROSPoint32(2, 2, 0)
  point9 = ROSPoint32(1, 2, 0)
  point10 = ROSPoint32(-2, 1, 0)
  point11 = ROSPoint32(-2, 2, 0)

  polygon0 = ROSPolygon()
  polygon0.points.append(point0)
  polygon0.points.append(point1)
  polygon0.points.append(point2)
  polygon0.points.append(point3)

  polygon1 = ROSPolygon()
  polygon1.points.append(point4)
  polygon1.points.append(point0)
  polygon1.points.append(point3)
  polygon1.points.append(point5)

  polygon2 = ROSPolygon()
  polygon2.points.append(point6)
  polygon2.points.append(point7)
  polygon2.points.append(point8)
  polygon2.points.append(point9)

  polygon3 = ROSPolygon()
  polygon3.points.append(point10)
  polygon3.points.append(point6)
  polygon3.points.append(point9)
  polygon3.points.append(point11)

  polygon2dmodel = Polygon2DModel()
  polygon2dmodel.type = '2dpolygon'
  polygon2dmodel.pose = ROSPose()
  polygon2dmodel.geometry = polygon0

  point3dmodel = Point3DModel()
  point3dmodel.type = '3dpoint'
  point3dmodel.geometry.x = 2.0
  point3dmodel.geometry.y = 2.0
  point3dmodel.geometry.z = 1.0

  pose3dmodel = Pose3DModel()
  pose3dmodel.type = '3dpose'
  pose3dmodel.pose = ROSPose()
  pose3dmodel.pose.position.x = -0.5
  pose3dmodel.pose.position.y = -0.5
  pose3dmodel.pose.position.z = 0.5

  rand_quat = random_quaternion()
  pose3dmodel.pose.orientation.x = rand_quat[0]
  pose3dmodel.pose.orientation.y = rand_quat[1]
  pose3dmodel.pose.orientation.z = rand_quat[2]
  pose3dmodel.pose.orientation.w = rand_quat[3]

  polygon3dmodel = Polygon3DModel()
  polygon3dmodel.type = '3dpolygon'
  polygon3dmodel.pose = ROSPose()
  polygon3dmodel.pose.position.x = 2.0
  polygon3dmodel.geometry = polygon1

  tri0 = ROSMeshTriangle()
  tri1 = ROSMeshTriangle()
  tri0.vertex_indices[0] = 0
  tri0.vertex_indices[1] = 1
  tri0.vertex_indices[2] = 2

  tri1.vertex_indices[0] = 0
  tri1.vertex_indices[1] = 2
  tri1.vertex_indices[2] = 3

  tpoint0 = ROSPoint( 0.0, 0.0, 0.0)
  tpoint1 = ROSPoint( 1.0, 0.0, 1.0)
  tpoint2 = ROSPoint( 1.0, 1.0, 0.0)
  tpoint3 = ROSPoint( 0.0, 1.0, 1.0)
  tpoint4 = ROSPoint(-1.0, 0.0, 0.0)
  tpoint5 = ROSPoint(-1.0, 1.0, 1.0)

  trianglemesh3dmodel = TriangleMesh3DModel()
  trianglemesh3dmodel.type = '3dtrianglemesh'
  trianglemesh3dmodel.pose = ROSPose()
  rand_quat = random_quaternion()
  trianglemesh3dmodel.pose.position.x = 1.0
  trianglemesh3dmodel.pose.position.y = -2.0
  trianglemesh3dmodel.pose.position.z = 0.25
  trianglemesh3dmodel.pose.orientation.x = 0.0
  trianglemesh3dmodel.pose.orientation.y = 0.0
  trianglemesh3dmodel.pose.orientation.z = 0.0
  trianglemesh3dmodel.pose.orientation.w = 0.0
  trianglemesh3dmodel.geometry.vertices.append(tpoint0)
  trianglemesh3dmodel.geometry.vertices.append(tpoint1)
  trianglemesh3dmodel.geometry.vertices.append(tpoint2)
  trianglemesh3dmodel.geometry.vertices.append(tpoint3)
  trianglemesh3dmodel.geometry.triangles.append(tri0)
  trianglemesh3dmodel.geometry.triangles.append(tri1)

  polygonmesh3dmodel = PolygonMesh3DModel()
  polygonmesh3dmodel.type = '3dpolygonmesh'
  polygonmesh3dmodel.pose = ROSPose()
  rand_quat = random_quaternion()
  polygonmesh3dmodel.pose.position.x = 0.5
  polygonmesh3dmodel.pose.position.y = 1.0
  polygonmesh3dmodel.pose.position.z = 0.0
  polygonmesh3dmodel.pose.orientation.x = 0.0
  polygonmesh3dmodel.pose.orientation.y = 0.0
  polygonmesh3dmodel.pose.orientation.z = 0.0
  polygonmesh3dmodel.pose.orientation.w = 0.0
  polygonmesh3dmodel.geometry.polygons.append(polygon2)
  polygonmesh3dmodel.geometry.polygons.append(polygon3)

  desc_ros = ROSObjectDescription()
  desc_ros.type = "Geometry"
  desc_ros.point2d_models.append(point2dmodel)
  desc_ros.pose2d_models.append(pose2dmodel)
  desc_ros.polygon2d_models.append(polygon2dmodel)
  desc_ros.point3d_models.append(point3dmodel)
  desc_ros.pose3d_models.append(pose3dmodel)
  desc_ros.polygon3d_models.append(polygon3dmodel)
  desc_ros.trianglemesh3d_models.append(trianglemesh3dmodel)
  desc_ros.polygonmesh3d_models.append(polygonmesh3dmodel)

  return desc_ros