def generatePolygons(header, model_index):
    polygon = PolygonArray()
    polygon.header = header
    polygon.polygons = [PolygonStamped()]
    polygon.polygons[0].header = header;
    coef = ModelCoefficientsArray()
    coef.coefficients = [ModelCoefficients()]
    coef.header = header
    if model_index in [0, 1, 2, 3]:
        # Rectangle
        polygon.polygons[0].polygon.points = [Point32(x=1.0, y=1.0),
                                              Point32(x=-1.0, y=1.0),
                                              Point32(x=-1.0, y=-1.0),
                                              Point32(x=1.0, y=-1.0)]
        coef.coefficients[0].header = header
        coef.coefficients[0].values = [0, 0, 1, 0]
    elif model_index == 4:
        polygon.polygons[0].polygon.points = [Point32(x=0.0, y=-0.5, z=0.0),
                                              Point32(x=0.0, y=-0.5, z=2.0),
                                              Point32(x=0.0, y=0.5, z=2.0),
                                              Point32(x=0.0, y=0.5, z=0.0)]
        # polygon.polygons[0].polygon.points.reverse()
        coef.coefficients[0].header = header
        coef.coefficients[0].values = [-1, 0, 0, 0]
    return (polygon, coef)
Example #2
0
def timer_cb(msg1, msg2, msg3):
    global polygons
    # print(msg1.header)
    # info_msg_polygon=msg2
    info_msg_polygon = PolygonArray()
    # info_msg_coefficient=msg3
    info_msg_coefficient = ModelCoefficientsArray()
    planes_indices = msg1.cluster_indices
    # print('len(planes_indices)', len(planes_indices))
    ind = 0
    longest_ind = 0
    longest_length = 0
    if (len(planes_indices) >= 1):
        for ind, plane in enumerate(planes_indices):
            length = len(plane.indices)
            if (length > longest_length):
                longest_ind = ind
                longest_length = length
                ind += 1
        # print(longest_ind)
        # # print (msg2.polygons)
        # print (len(msg2.polygons))
        # print (len(planes_indices))
        info_msg_polygon.polygons = msg2.polygons
        info_msg_polygon.header = msg2.header
        info_msg_coefficient.coefficients = [msg3.coefficients[longest_ind]]
        info_msg_coefficient.header = msg3.header
        pub_info_polygon.publish(info_msg_polygon)
        pub_info_coefficient.publish(info_msg_coefficient)
        # print('len(msg2.polygons)', len(msg2.polygons))
        polygons = msg2.polygons[longest_ind]
    if polygons is not None:
        pub_info_polygonstamped.publish(polygons)
    else:
        print("finding planes")
Example #3
0
def generatePolygons(header, model_index):
    polygon = PolygonArray()
    polygon.header = header
    polygon.polygons = [PolygonStamped()]
    polygon.polygons[0].header = header
    coef = ModelCoefficientsArray()
    coef.coefficients = [ModelCoefficients()]
    coef.header = header
    if model_index in [0, 1, 2, 3]:
        # Rectangle
        polygon.polygons[0].polygon.points = [
            Point32(x=1.0, y=1.0),
            Point32(x=-1.0, y=1.0),
            Point32(x=-1.0, y=-1.0),
            Point32(x=1.0, y=-1.0)
        ]
        coef.coefficients[0].header = header
        coef.coefficients[0].values = [0, 0, 1, 0]
    elif model_index == 4:
        polygon.polygons[0].polygon.points = [
            Point32(x=0.0, y=-0.5, z=0.0),
            Point32(x=0.0, y=-0.5, z=2.0),
            Point32(x=0.0, y=0.5, z=2.0),
            Point32(x=0.0, y=0.5, z=0.0)
        ]
        # polygon.polygons[0].polygon.points.reverse()
        coef.coefficients[0].header = header
        coef.coefficients[0].values = [-1, 0, 0, 0]
    return (polygon, coef)
Example #4
0
def sub_bb(msg):
    rospy.loginfo("sub_bb cb")
    bb = msg.boxes[0]
    x1 = bb.pose.position.x - bb.dimensions.x
    x2 = bb.pose.position.x + bb.dimensions.x
    y1 = bb.pose.position.y - bb.dimensions.y
    y2 = bb.pose.position.y + bb.dimensions.y
    z1 = bb.pose.position.z - bb.dimensions.z
    z2 = bb.pose.position.z + bb.dimensions.z
    ps = PolygonStamped()
    if face == "xy":
        ps.polygon.points.append(Point32(x1, y1, z1))
        ps.polygon.points.append(Point32(x1, y2, z1))
        ps.polygon.points.append(Point32(x2, y2, z1))
        ps.polygon.points.append(Point32(x2, y1, z1))
    elif face == "yz":
        ps.polygon.points.append(Point32(x1, y1, z1))
        ps.polygon.points.append(Point32(x1, y1, z2))
        ps.polygon.points.append(Point32(x1, y2, z2))
        ps.polygon.points.append(Point32(x1, y2, z1))
    elif face == "zx":
        ps.polygon.points.append(Point32(x1, y1, z1))
        ps.polygon.points.append(Point32(x1, y1, z2))
        ps.polygon.points.append(Point32(x2, y1, z2))
        ps.polygon.points.append(Point32(x2, y1, z1))
    else:
        rospy.loginfo("Face Error!")
    ps.header = msg.header
    pa = PolygonArray()
    pa.header = msg.header
    pa.polygons = [ps]
    rospy.loginfo('pa:%s', pa)
    pub.publish(pa)
Example #5
0
    def pathLocalToPolArr(self):
        pa = PolygonArray()
        pa.header.stamp = rospy.Time.now()
        pa.header.frame_id = "map"
        for i in range(self.N-1):

            spoly = np.array([self.pathlocal.s[i], self.pathlocal.s[i+1],self.pathlocal.s[i+1],self.pathlocal.s[i]])
            dpoly = np.array([self.pathlocal.dub[i], self.pathlocal.dub[i+1],self.pathlocal.dlb[i+1],self.pathlocal.dlb[i]])
            Xpoly, Ypoly = ptsFrenetToCartesian(spoly,dpoly,self.pathlocal.X,self.pathlocal.Y,self.pathlocal.psi_c,self.pathlocal.s)

            p = PolygonStamped()
            p.header.stamp = rospy.Time.now()
            p.header.frame_id = "map"
            p.polygon.points = [Point32(x=Xpoly[0], y=Ypoly[0], z=0.0),
                                Point32(x=Xpoly[1], y=Ypoly[1], z=0.0),
                                Point32(x=Xpoly[2], y=Ypoly[2], z=0.0),
                                Point32(x=Xpoly[3], y=Ypoly[3], z=0.0)]


            pa.polygons.append(p)
            #pa.Color.r = 1.0
            #p.color.g = 0.0
            #p.color.b = 0.0        
        
        # color for mu
        pa.likelihood = self.pathlocal.mu[0:-1]*(0.2/np.max(self.pathlocal.mu)) # discarding final value
               
        #pa.color.r = 1.0
        #pa.color.g = 0.0
        #pa.color.b = 0.0
        
        return pa
Example #6
0
 def _convert(self, msg):
     polys_msg = PolygonArray()
     polys_msg.header = msg.header
     for rect in msg.rects:
         poly_msg = PolygonStamped()
         poly_msg.header = msg.header
         pt0 = Point32(x=rect.x, y=rect.y)
         pt1 = Point32(x=rect.x + rect.width, y=rect.y + rect.height)
         poly_msg.polygon.points.append(pt0)
         poly_msg.polygon.points.append(pt1)
         polys_msg.polygons.append(poly_msg)
     self.pub.publish(polys_msg)
 def _convert(self, msg):
     polys_msg = PolygonArray()
     polys_msg.header = msg.header
     for rect in msg.rects:
         poly_msg = PolygonStamped()
         poly_msg.header = msg.header
         pt0 = Point32(x=rect.x, y=rect.y)
         pt1 = Point32(x=rect.x + rect.width, y=rect.y + rect.height)
         poly_msg.polygon.points.append(pt0)
         poly_msg.polygon.points.append(pt1)
         polys_msg.polygons.append(poly_msg)
     self.pub.publish(polys_msg)
 def generate_plane(self, header,
                    xmin=-0.0, xmax=1.0,
                    ymin=-1.0, ymax=1.0):
     msg = PolygonArray()
     msg.header = header
     p = PolygonStamped()
     p.header = header
     p.polygon.points = [Point32(x=xmin, y=ymin),
                         Point32(x=xmax, y=ymin),
                         Point32(x=xmax, y=ymax),
                         Point32(x=xmin, y=ymax)]
     msg.polygons.append(p)
     return msg
 def publishPolygons(self, event=None):
     pmsg = PolygonArray()
     pmsg.header.stamp = rospy.Time.now()
     cmsg = ModelCoefficientsArray()
     cmsg.header.stamp = pmsg.header.stamp
     for i in range(10):
         pmsg.polygons.append(PolygonStamped())
         pmsg.polygons[i].header.stamp = pmsg.header.stamp
         pmsg.polygons[i].header.frame_id = str(i)
         cmsg.coefficients.append(ModelCoefficients())
         cmsg.coefficients[i].header.stamp = cmsg.header.stamp
         cmsg.coefficients[i].header.frame_id = str(i)
     pmsg.likelihood = [1.0,2.0,3.0,4.0,5.0,4.0,3.0,2.0,1.0,0.0]
     self.pub_polygon.publish(pmsg)
     self.pub_coefficients.publish(cmsg)
Example #10
0
 def publishPolygons(self, event=None):
     pmsg = PolygonArray()
     pmsg.header.stamp = rospy.Time.now()
     cmsg = ModelCoefficientsArray()
     cmsg.header.stamp = pmsg.header.stamp
     for i in range(10):
         pmsg.polygons.append(PolygonStamped())
         pmsg.polygons[i].header.stamp = pmsg.header.stamp
         pmsg.polygons[i].header.frame_id = str(i)
         cmsg.coefficients.append(ModelCoefficients())
         cmsg.coefficients[i].header.stamp = cmsg.header.stamp
         cmsg.coefficients[i].header.frame_id = str(i)
     pmsg.likelihood = [1.0, 2.0, 3.0, 4.0, 5.0, 4.0, 3.0, 2.0, 1.0, 0.0]
     self.pub_polygon.publish(pmsg)
     self.pub_coefficients.publish(cmsg)
Example #11
0
    def parse_params(self, param, frame_id):
        # validate params
        assert isinstance(param, list), "polygons must be list"

        has_label = any("label" in p for p in param)
        has_likelihood = any("likelihood" in p for p in param)

        for polygon in param:
            assert "points" in polygon and isinstance(polygon["points"], list),\
                   "each polygon must have at least 1 'points'"
            for point in polygon["points"]:
                assert len(point) == 3,\
                       "element of 'points' must be list of 3 numbers"
            if has_label:
                assert "label" in polygon, "missing 'label'"
            if has_likelihood:
                assert "likelihood" in polygon, "missing 'likelihood'"

        # parse params
        msg = PolygonArray()
        msg.header.frame_id = frame_id
        for polygon in param:
            ps = PolygonStamped()
            ps.header.frame_id = frame_id
            ps.polygon.points = [Point32(*v) for v in polygon["points"]]
            msg.polygons.append(ps)
            if has_label:
                msg.labels.append(polygon["label"])
            if has_likelihood:
                msg.likelihood.append(polygon["likelihood"])

        return msg
Example #12
0
def makePolygonArray():
    global_frame_id = rospy.myargv()[1]
    ret = PolygonArray()
    ret.header.stamp = rospy.Time.now()
    ret.header.frame_id = global_frame_id

    for i in range(-1, 5):
        # i = 0,  1, 2, 3, 4, 5
        x1 = i * 3 - 0.5
        x2 = i * 3 + 1 - 0.5
        x3 = i * 3 + 2 - 0.5
        x4 = i * 3 + 3 - 0.5
        points_array = [[[x2, 2, 0], [x1, 2, 0], [x1, -2, 0], [x2, -2, 0]],
                        [[x3, 2, 0.3], [x2, 2, 0.2], [x2, -2, 0.2],
                         [x3, -2, 0.3]],
                        [[x4, 2, 0.1], [x3, 2, 0.1], [x3, -2, 0.1],
                         [x4, -2, 0.1]]]
        for points in points_array:
            polygon = PolygonStamped()
            polygon.header.frame_id = global_frame_id
            polygon.header.stamp = rospy.Time.now()
            for p in points:
                polygon.polygon.points.append(Point32(*p))
            ret.polygons.append(polygon)
    return ret
Example #13
0
def getting_cordi(A,B):
	theta_increment=(2)*(math.pi)/360
	re=PolygonArray()
	re.header.frame_id="odom"
	roar=PolygonStamped()
	roar.header.frame_id="odom"
	pt=[]	
	count=0
#	print("This is A:")
#	print(A)
	for i in range(len(A)):
		if str(A[i])!="inf":
			count+=1
			theta1=(i)*theta_increment
#			print(A[i])
			x=A[i]*math.sin(theta1)
			y=A[i]*math.cos(theta1)       
			roar.polygon.points.append(Point32(x,y,0))
			pt.append([x,y,theta1,i])
			if i==len(A)-1 or str(A[i+1])=="inf":
				for j in range(i,i-count,-1):
					theta2=(j)*theta_increment
					x=B[j]*math.sin(theta2)
					y=B[j]*math.cos(theta2)       
					roar.polygon.points.append(Point32(x,y,0))
					pt.append([x,y,theta2,j])			
#				print(pt)
				pt=[]
				count=0
#				print("***************************************************************")
#				print(poly.points)
				re.polygons.append(roar)
				roar=PolygonStamped()
				roar.header.frame_id="odom"
	return re		
Example #14
0
def getting_cordi(A,B,shu):
	theta_increment=shu
	re=PolygonArray()
	re.header.frame_id="base_scan"
	roar=PolygonStamped()
	roar.header.frame_id="base_scan"
	roar.header.stamp=rospy.Time.now()
	pt=Exp_msg()
	count=0
	baby=Ipoly()
	for i in range(len(A)):
		if str(A[i])!="inf":
			if count==0:
				for t in range(3,1,-1):
					theta1=(i-t)*theta_increment
					x=A[i]*math.cos(theta1)
					y=A[i]*math.sin(theta1)       
					roar.polygon.points.append(Point32(x,y,0))
					pt.bliss.append(Cordi(x,y,0))
			count+=1
			theta1=(i)*theta_increment
			x=A[i]*math.cos(theta1)
			y=A[i]*math.sin(theta1)       
			roar.polygon.points.append(Point32(x,y,0))
			pt.bliss.append(Cordi(x,y,0))
			if i==len(A)-1 or str(A[i+1])=="inf" or abs(A[i+1]-A[i])>0.1:
				for t in range(1,4):
					theta1=(t+i)*theta_increment
					x=A[i]*math.cos(theta1)
					y=A[i]*math.sin(theta1)       
					roar.polygon.points.append(Point32(x,y,0))
					pt.bliss.append(Cordi(x,y,0))
				for t in range(3,1,-1):
					theta1=(t+i)*theta_increment
					x=B[i]*math.cos(theta1)
					y=B[i]*math.sin(theta1)       
					roar.polygon.points.append(Point32(x,y,0))
					pt.bliss.append(Cordi(x,y,0))
				for j in range(i,i-count,-1):
					theta2=(j)*theta_increment
					x=B[j]*math.cos(theta2)
					y=B[j]*math.sin(theta2)       
					roar.polygon.points.append(Point32(x,y,0))
					pt.bliss.append(Cordi(x,y,0))
				for t in range(-1,-4,-1):
					theta2=(j+t)*theta_increment
					x=B[j]*math.cos(theta2)
					y=B[j]*math.sin(theta2)       
					roar.polygon.points.append(Point32(x,y,0))
					pt.bliss.append(Cordi(x,y,0))			
				baby.eternal_bliss.append(pt)
				pt=Exp_msg()
				count=0
				re.polygons.append(roar)
				roar=PolygonStamped()
				roar.header.frame_id="base_scan"
				roar.header.stamp=rospy.Time.now()
	pubf=rospy.Publisher("ol2",Ipoly,queue_size=0)
	pubf.publish(baby)
	return re		
Example #15
0
def getting_cordi(A, B, shu):
    theta_increment = shu
    re = PolygonArray()
    re.header.frame_id = "odom"
    roar = PolygonStamped()
    roar.header.frame_id = "odom"
    roar.header.stamp = rospy.Time.now()
    pt = Exp_msg()
    count = 0
    baby = Ipoly()
    for i in range(len(A)):
        if str(A[i]) != "inf":
            if count == 0:
                for t in range(10, 1, -1):
                    theta1 = (i - t) * theta_increment
                    x = A[i] * math.cos(theta1)
                    y = A[i] * math.sin(theta1)
                    roar.polygon.points.append(Point32(x, y, 0))
                    pt.bliss.append(Cordi(x, y, 0))
            count += 1
            theta1 = (i) * theta_increment
            x = A[i] * math.cos(theta1)
            y = A[i] * math.sin(theta1)
            roar.polygon.points.append(Point32(x, y, 0))
            pt.bliss.append(Cordi(x, y, 0))
            if i == len(A) - 1 or abs(A[i + 1] - A[i]) > 0.15:
                # ra=A[i]/math.cos(theta_increment)
                for t in range(1, 10):
                    theta1 = (t + i) * theta_increment
                    # ra=ra/math.cos(theta_increment)
                    x = A[i] * math.cos(theta1)
                    y = A[i] * math.sin(theta1)
                    roar.polygon.points.append(Point32(x, y, 0))
                    pt.bliss.append(Cordi(x, y, 0))
                for t in range(10, 1, -1):
                    theta1 = (t + i) * theta_increment
                    x = (B[i]) * math.cos(theta1)
                    y = (B[i]) * math.sin(theta1)
                    roar.polygon.points.append(Point32(x, y, 0))
                    pt.bliss.append(Cordi(x, y, 0))
                for j in range(i, i - count, -1):
                    theta2 = (j) * theta_increment
                    x = B[j] * math.cos(theta2)
                    y = B[j] * math.sin(theta2)
                    roar.polygon.points.append(Point32(x, y, 0))
                    pt.bliss.append(Cordi(x, y, 0))
                for t in range(-1, -10, -1):
                    theta2 = (j + t) * theta_increment
                    x = B[j] * math.cos(theta2)
                    y = B[j] * math.sin(theta2)
                    roar.polygon.points.append(Point32(x, y, 0))
                    pt.bliss.append(Cordi(x, y, 0))
                baby.eternal_bliss.append(pt)
                pt = Exp_msg()
                count = 0
                re.polygons.append(roar)
                roar = PolygonStamped()
                roar.header.frame_id = "odom"
                roar.header.stamp = rospy.Time.now()
    return re
Example #16
0
    def Spawner(self):

        polygon_array = PolygonArray()
        polygon_array.header.stamp = rospy.Time.now()
        polygon_array.header.frame_id = "map"

        for i, polygon_poses in enumerate(
                self.rviz_polygon_array_def["polygon_array_poses"]):
            polygon = PolygonStamped()
            polygon.header.stamp = rospy.Time.now()
            polygon.header.frame_id = "map"

            for pose in polygon_poses:
                polygon.polygon.points.append(
                    Point32(pose.position.x, pose.position.y, pose.position.z))
                polygon_array.labels.append(1)
                polygon_array.likelihood.append(1.0)

            polygon_array.polygons.append(polygon)

            # polygon_array.labels = self.rviz_polygon_array_def["labels"]#[i]
            # polygon_array.likelihood = self.rviz_polygon_array_def["likelihood"]#[i]

        # polygon_array.labels = [1,1,1,1,1,1]
        # polygon_array.likelihood = [1.0,1.0,1.0,1.0,1.0,1.0]

        self.polygon_array = polygon_array

        t = 0
        while not rospy.is_shutdown() and t < 10:
            self.polygon_array_pub.publish(self.polygon_array)
            t = t + 1
            time.sleep(0.05)
def getting_cordi(A,B,shu):
	theta_increment=shu
	re=PolygonArray()
	re.header.frame_id="odom"
	roar=PolygonStamped()
	roar.header.frame_id="odom"
	pt=[]	
	count=0
#	print("This is A:")
#	print(A)
	for i in range(len(A)):
		if str(A[i])!="inf":
			if count==0:
				for t in range(3,1,-1):
					theta1=(i-t)*theta_increment
		#			print(A[i])
					x=A[i]*math.cos(theta1)
					y=A[i]*math.sin(theta1)       
					roar.polygon.points.append(Point32(x,y,0))
	
			count+=1
			theta1=(i)*theta_increment
#			print(A[i])
			x=A[i]*math.cos(theta1)
			y=A[i]*math.sin(theta1)       
			roar.polygon.points.append(Point32(x,y,0))
			pt.append([x,y,theta1,i])
			if i==len(A)-1 or str(A[i+1])=="inf" or abs(A[i+1]-A[i])>0.5:
				for t in range(1,5):
					theta1=(t+i)*theta_increment
					x=A[i]*math.cos(theta1)
					y=A[i]*math.sin(theta1)       
					roar.polygon.points.append(Point32(x,y,0))
				for t in range(3,0,-1):
					theta1=(t+i)*theta_increment
					x=B[i]*math.cos(theta1)
					y=B[i]*math.sin(theta1)       
					roar.polygon.points.append(Point32(x,y,0))
				for j in range(i,i-count,-1):
					theta2=(j)*theta_increment
					x=B[j]*math.cos(theta2)
					y=B[j]*math.sin(theta2)       
					roar.polygon.points.append(Point32(x,y,0))
					pt.append([x,y,theta2,j])
				for t in range(-1,-3,-1):
					theta2=(j+t)*theta_increment
					x=B[j]*math.cos(theta2)
					y=B[j]*math.sin(theta2)       
					roar.polygon.points.append(Point32(x,y,0))			
#				print(pt)
				pt=[]
				count=0
#				print("***************************************************************")
#				print(poly.points)
				re.polygons.append(roar)
				roar=PolygonStamped()
				roar.header.frame_id="odom"
	return re		
 def callback(self, msg, msg_coef):
     if len(msg.polygons) > 0:
         #self._pub.publish(msg.histograms[0])
         max_index = max(xrange(len(msg.polygons)), key=lambda i: msg.likelihood[i])
         res = PolygonArray()
         res.header = msg.header
         res.polygons = [msg.polygons[max_index]]
         res.likelihood = [msg.likelihood[max_index]]
         if msg.likelihood[max_index] < self._min_likelihood:
             rospy.loginfo("Ignore result because of too small likelihood: {0} < {1}".format(
                 msg.likelihood[max_index],
                 self._min_likelihood))
             return
         self._pub.publish(res)
         res_coef = ModelCoefficientsArray()
         res_coef.header = msg.header
         res_coef.coefficients = [msg_coef.coefficients[max_index]]
         self._pub_coef.publish(res_coef)
def callback(polygon_msg, coeff_msg):
    print "callback"
    # odom->ground
    max_val = -1.0;
    max_index = None
    for i in range(len(polygon_msg.polygons)):
        val = polygon_msg.likelihood
        if max_val < val:
            max_val = val
            max_index = i
    if max_index != None:
        out_poly_msg = PolygonArray()
        out_poly_msg.header = polygon_msg.header
        out_poly_msg.polygons = [polygon_msg.polygons[max_index]]
        pub_poly.publish(out_poly_msg)
        out_coeff_msg = ModelCoefficientsArray()
        out_coeff_msg.header = coeff_msg.header
        out_coeff_msg.coefficients= [coeff_msg.coefficients[max_index]]
        pub_coeff.publish(out_coeff_msg)
def callback(polygon_msg, coeff_msg):
    print "callback"
    # odom->ground
    max_val = -1.0
    max_index = None
    for i in range(len(polygon_msg.polygons)):
        val = polygon_msg.likelihood
        if max_val < val:
            max_val = val
            max_index = i
    if max_index != None:
        out_poly_msg = PolygonArray()
        out_poly_msg.header = polygon_msg.header
        out_poly_msg.polygons = [polygon_msg.polygons[max_index]]
        pub_poly.publish(out_poly_msg)
        out_coeff_msg = ModelCoefficientsArray()
        out_coeff_msg.header = coeff_msg.header
        out_coeff_msg.coefficients = [coeff_msg.coefficients[max_index]]
        pub_coeff.publish(out_coeff_msg)
Example #21
0
 def callback(self, msg, msg_coef):
     if len(msg.polygons) > 0:
         #self._pub.publish(msg.histograms[0])
         max_index = max(xrange(len(msg.polygons)),
                         key=lambda i: msg.likelihood[i])
         res = PolygonArray()
         res.header = msg.header
         res.polygons = [msg.polygons[max_index]]
         res.likelihood = [msg.likelihood[max_index]]
         if msg.likelihood[max_index] < self._min_likelihood:
             rospy.loginfo(
                 "Ignore result because of too small likelihood: {0} < {1}".
                 format(msg.likelihood[max_index], self._min_likelihood))
             return
         self._pub.publish(res)
         res_coef = ModelCoefficientsArray()
         res_coef.header = msg.header
         res_coef.coefficients = [msg_coef.coefficients[max_index]]
         self._pub_coef.publish(res_coef)
    def dynamic_custom_polygon_demo(self, zones_list):

        #rospy.init_node("polygon_array_dynamic")
        # /polygon_array_dynamic/output
        pub = rospy.Publisher("/polygon_array_dynamic/output",
                              PolygonArray,
                              queue_size=1)
        r = rospy.Rate(5)
        count = 0
        msg = PolygonArray()
        header = Header()
        header.frame_id = "world"
        header.stamp = rospy.Time.now()
        msg.header = header
        msg.polygons = []
        msg.labels = []
        msg.likelihood = []
        for i in zones_list:
            #print(i)
            msg.polygons.append(self.DynamicPolygon(header, i))
            msg.labels.append(count)
            msg.likelihood.append(np.random.ranf())
            count += 1

        rospy.loginfo("Publishing box")
        pub.publish(msg)
        r.sleep()
Example #23
0
def sample_demo():
    """
    Draws a star, circle, rectangle and square 
    """
    rospy.init_node("polygon_array_sample")
    pub = rospy.Publisher("~output", PolygonArray, queue_size=1)
    r = rospy.Rate(10)
    while not rospy.is_shutdown():
        msg = PolygonArray()
        header = Header()
        header.frame_id = "world"
        header.stamp = rospy.Time.now()
        msg.header = header
        msg.polygons = [
            SquarePolygon(header),
            RectanglePolygon(header),
            CirclePolygon(header),
            StarPolygon(header)
        ]
        # rospy.loginfo("number of point to enter :"+ str(msg.polygons))
        msg.labels = [0, 1, 2, 3]
        msg.likelihood = [
            np.random.ranf(),
            np.random.ranf(),
            np.random.ranf(),
            np.random.ranf()
        ]
        pub.publish(msg)
        r.sleep()
Example #24
0
def callback(msg):
    output_msg = PolygonArray()
    output_msg.header = msg.header
    output_msg.polygons = msg.polygons
    output_msg.labels = msg.labels
    output_msg.likelihood = msg.likelihood

    # fill empty
    output_msg.header.stamp = rospy.Time(0)
    for i in range(len(output_msg.polygons)):
        output_msg.polygons[i].header = output_msg.header

    pub.publish(output_msg)
def imu_cb(imu):
    ax, ay, az = imu.linear_acceleration.x, imu.linear_acceleration.y, imu.linear_acceleration.z
    # rospy.loginfo("%f %f %f" % (ax, ay, az))
    if az > 0.1:
        dx, dy = [-az, 0, ax], [0, -az, ay]
    elif ay > 0.1:
        dx, dy = [-ay, ax, 0], [0, az, -ay]
    else:
        dx, dy = [ay, -ax, 0], [az, 0, -az]
    PStamped = PolygonStamped()
    for x, y in [[-10, -10], [-10, 10], [10, 10], [10, -10]]:
        PStamped.polygon.points.append(
            Point32(x * dx[0] + y * dy[0], x * dx[1] + y * dy[1],
                    x * dx[2] + y * dy[2]))

    PStamped.header = imu.header
    PArrayPub.publish(PolygonArray(imu.header, [PStamped]))
    normal_array = np.array([ax, ay, az, 0])
    normal_array = normal_array / np.linalg.norm(x)
    MStamped = ModelCoefficients(imu.header, normal_array)
    MArrayPub.publish(ModelCoefficientsArray(imu.header, [MStamped]))
Example #26
0
def dynamic_custom_polygon_demo():

    rospy.init_node("polygon_array_dynamic")
    # /polygon_array_dynamic/output
    pub = rospy.Publisher("~output", PolygonArray, queue_size=1)
    r = rospy.Rate(5)
    #while not rospy.is_shutdown():
    msg = PolygonArray()
    header = Header()
    header.frame_id = "world"
    header.stamp = rospy.Time.now()
    msg.header = header
    msg.polygons = [DynamicPolygon(header)]
    msg.labels = [0]
    msg.likelihood = [np.random.ranf()]
    pub.publish(msg)
    r.sleep()
Example #27
0
    def callback(self, point_cloud):

        print("callback")

        points = np.array(list(pc2.read_points(point_cloud, skip_nans=True)))
        z_points = points[:, [2]]
        self.max_z = max(z_points)

        points_surface = points[:, [0, 1]]  # choose x, y

        points_surface = points_surface[np.argsort(
            points_surface[:, 0])]  # sort x

        min_x = points_surface[0, 0]
        max_x = points_surface[-1, 0]

        points_surface = points_surface[np.argsort(
            points_surface[:, 1])]  # sort x

        min_y = points_surface[0, 1]
        max_y = points_surface[-1, 1]

        self.image_pixel_zero_pos = [min_x, min_y]

        pixel_size_x = ((max_x - min_x) / self.one_pixel_length)
        pixel_size_y = ((max_y - min_y) / self.one_pixel_length)

        pixel_size_x_int = int((max_x - min_x) / self.one_pixel_length)
        pixel_size_y_int = int((max_y - min_y) / self.one_pixel_length)

        if ((pixel_size_x - pixel_size_x_int) >= 0.5):
            pixel_size_x_int = pixel_size_x_int + 1
        if ((pixel_size_y - pixel_size_y_int) >= 0.5):
            pixel_size_y_int = pixel_size_y_int + 1

        np_image = np.zeros((pixel_size_x_int, pixel_size_y_int, 3))
        print("image size:", pixel_size_x_int, pixel_size_y_int)

        for i in points_surface:
            x = i[0]
            y = i[1]

            pix_x = (x - self.image_pixel_zero_pos[0]) / self.one_pixel_length
            pix_y = (y - self.image_pixel_zero_pos[1]) / self.one_pixel_length

            pix_x_int = int(
                (x - self.image_pixel_zero_pos[0]) / self.one_pixel_length)
            pix_y_int = int(
                (y - self.image_pixel_zero_pos[1]) / self.one_pixel_length)

            if (pix_x - pix_x_int >= 0.5):
                pix_x_int = pix_x_int + 1
            if (pix_y - pix_y_int >= 0.5):
                pix_y_int = pix_y_int + 1

            np_image[pix_x_int - 1][pix_y_int - 1] = [255, 255, 255]

        img = np_image.astype(np.uint8)

        kernel = np.ones((5, 5), np.uint8)
        img = cv2.morphologyEx(img, cv2.MORPH_CLOSE, kernel)

        gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
        _, bw = cv2.threshold(gray, 0, 255,
                              cv2.THRESH_BINARY | cv2.THRESH_OTSU)
        ret = findSquares(bw, img)

        img_polygons = ret[0]
        img_centers = ret[1]

        polygons = []
        centers = []

        for i in range(len(img_polygons)):
            polygon = []
            for j in range(len(img_polygons[0])):
                pos_xy = self.pixel_to_pos(img_polygons[i][j])
                polygon.append([pos_xy[0], pos_xy[1], self.max_z])

            polygons.append(polygon)

            pos_xy = self.pixel_to_pos(img_centers[i])
            centers.append([pos_xy[0], pos_xy[1], self.max_z])

        header = point_cloud.header

        msg = PolygonArray()
        msg.header = header

        for i in range(len(polygons)):
            p = PolygonStamped()
            p.header = header
            for j in range(len(polygons[0])):
                xyz = polygons[i][j]
                p.polygon.points.append(Point32(x=xyz[0], y=xyz[1], z=xyz[2]))

            msg.polygons.append(p)

        self.pub_polygons.publish(msg)

        pub_msg = PoseArray()
        pub_msg.header = point_cloud.header

        for i in range(len(centers)):
            pose = Pose()
            pose.position.x = centers[i][0]
            pose.position.y = centers[i][1]
            pose.position.z = centers[i][2]

            pub_msg.poses.append(pose)

        self.pub_target_poses.publish(pub_msg)
def timer_cb(msg1):
    plarray = PolygonArray()
    plarray.header = msg1.header
    plarray.polygons = [msg1]
    pub_info_polygonstamped.publish(plarray)
Example #29
0
        ps.polygon.points.append(Point32(x2, y1, z1))
    elif face == "yz":
        ps.polygon.points.append(Point32(x1, y1, z1))
        ps.polygon.points.append(Point32(x1, y1, z2))
        ps.polygon.points.append(Point32(x1, y2, z2))
        ps.polygon.points.append(Point32(x1, y2, z1))
    elif face == "zx":
        ps.polygon.points.append(Point32(x1, y1, z1))
        ps.polygon.points.append(Point32(x1, y1, z2))
        ps.polygon.points.append(Point32(x2, y1, z2))
        ps.polygon.points.append(Point32(x2, y1, z1))
    else:
        rospy.loginfo("Face Error!")
    ps.header = msg.header
    pa = PolygonArray()
    pa.header = msg.header
    pa.polygons = [ps]
    rospy.loginfo('pa:%s', pa)
    pub.publish(pa)


if __name__ == '__main__':
    rospy.init_node("bounding_box_to_polygon_array")
    pub = rospy.Publisher("output", PolygonArray)
    rospy.Subscriber("/tablecloth_bb", BoundingBoxArray, sub_bb)
    r = rospy.Rate(10)
    bb = BoundingBox()
    pa = PolygonArray()
    face = rospy.get_param("face", "xy")
    rospy.spin()
Example #30
0
        Point32(x=-.3633, y=-.1180 + 3.0),
        Point32(x=-.9511, y=.3090 + 3.0),
        Point32(x=-.2245, y=.3090 + 3.0)
    ]
    return p


if __name__ == "__main__":
    rospy.init_node("polygon_array_sample")
    pub = rospy.Publisher("~output", PolygonArray)
    r = rospy.Rate(10)

    br = tf.TransformBroadcaster()

    while not rospy.is_shutdown():
        msg = PolygonArray()
        header = Header()
        header.frame_id = "world"
        header.stamp = rospy.Time.now()
        msg.header = header
        msg.polygons = [
            SquarePolygon(header),
            RectanglePolygon(header),
            CirclePolygon(header),
            StarPolygon(header)
        ]
        msg.labels = [0, 1, 2, 3]
        msg.likelihood = [
            np.random.ranf(),
            np.random.ranf(),
            np.random.ranf(),
    p.header = header
    p.polygon.points = [Point32(x= .0000, y= 1.0000 + 3.0),
                        Point32(x= .2245, y= .3090 + 3.0),
                        Point32(x= .9511, y= .3090 + 3.0),
                        Point32(x= .3633, y= -.1180 + 3.0),
                        Point32(x= .5878, y= -.8090 + 3.0),
                        Point32(x= .0000, y= -.3820 + 3.0),
                        Point32(x= -.5878, y= -.8090 + 3.0),
                        Point32(x= -.3633, y= -.1180 + 3.0),
                        Point32(x= -.9511, y= .3090 + 3.0),
                        Point32(x= -.2245, y= .3090 + 3.0)]
    return p
    
if __name__ == "__main__":
    rospy.init_node("polygon_array_sample")
    pub = rospy.Publisher("~output", PolygonArray)
    r = rospy.Rate(10)
    while not rospy.is_shutdown():
        msg = PolygonArray()
        header = Header()
        header.frame_id = "world"
        header.stamp = rospy.Time.now()
        msg.header = header
        msg.polygons = [SquarePolygon(header),
                        RectanglePolygon(header),
                        CirclePolygon(header),
                        StarPolygon(header)]
        msg.labels = [0, 1, 2, 3]
        msg.likelihood = [np.random.ranf(), np.random.ranf(), np.random.ranf(), np.random.ranf()]
        pub.publish(msg)
def getting_cordi(A, B, shu):
    theta_increment = shu
    re = PolygonArray()
    re.header.frame_id = "base_scan"
    roar = PolygonStamped()
    roar.header.frame_id = "base_scan"
    pt = Exp_msg()
    count = 0
    baby = Ipoly()
    for i in range(len(A)):
        if str(A[i]) != "inf":
            if count == 0:
                for t in range(3, 1, -1):
                    theta1 = (i - t) * theta_increment
                    x = A[i] * math.cos(theta1)
                    y = A[i] * math.sin(theta1)
                    roar.polygon.points.append(Point32(x, y, 0))
                    pt.bliss.append(Cordi(x, y, 0))
            count += 1
            theta1 = (i) * theta_increment
            x = A[i] * math.cos(theta1)
            y = A[i] * math.sin(theta1)
            roar.polygon.points.append(Point32(x, y, 0))
            pt.bliss.append(Cordi(x, y, 0))
            if i == len(A) - 1 or str(
                    A[i + 1]) == "inf" or abs(A[i + 1] - A[i]) > 0.5:
                for t in range(1, 5):
                    theta1 = (t + i) * theta_increment
                    x = A[i] * math.cos(theta1)
                    y = A[i] * math.sin(theta1)
                    roar.polygon.points.append(Point32(x, y, 0))
                    pt.bliss.append(Cordi(x, y, 0))
                for t in range(3, 0, -1):
                    theta1 = (t + i) * theta_increment
                    x = B[i] * math.cos(theta1)
                    y = B[i] * math.sin(theta1)
                    roar.polygon.points.append(Point32(x, y, 0))
                    pt.bliss.append(Cordi(x, y, 0))
                for j in range(i, i - count, -1):
                    theta2 = (j) * theta_increment
                    x = B[j] * math.cos(theta2)
                    y = B[j] * math.sin(theta2)
                    roar.polygon.points.append(Point32(x, y, 0))
                    pt.bliss.append(Cordi(x, y, 0))
                for t in range(-1, -3, -1):
                    theta2 = (j + t) * theta_increment
                    x = B[j] * math.cos(theta2)
                    y = B[j] * math.sin(theta2)
                    roar.polygon.points.append(Point32(x, y, 0))
                    pt.bliss.append(Cordi(x, y, 0))
                baby.eternal_bliss.append(pt)
                pt = Exp_msg()
                count = 0
                re.polygons.append(roar)
                roar = PolygonStamped()
                roar.header.frame_id = "base_scan"
    pubf = rospy.Publisher("ol2", Ipoly, queue_size=0)
    pubf.publish(baby)

    # print "bot yaw calculator ", bot_yaw

    final_polys = PolygonArray()
    # print "New"
    for p in re.polygons:
        # print " "
        temp_poly = PolygonStamped()
        temp_poly.header.frame_id = "odom"
        for point in p.polygon.points:
            temp_point = numpy.array([[point.x], [point.y]])
            # print temp_point
            # print point.x, point.y
            # print "bot yaw calculator ", bot_yaw
            cos = math.cos(-bot_yaw)
            sin = math.sin(-bot_yaw)
            # print cos, sin
            rot_mat = numpy.array([[cos, sin], [-sin, cos]])
            # rot_mat=numpy.array([[1 , 0],[0 , 1]])
            trans_mat = numpy.array([[bot_x], [bot_y]])

            rot_point = numpy.matmul(rot_mat, temp_point)
            # print rot_point
            trans_point = rot_point + trans_mat
            # print trans_point[0], trans_point[1]
            # point.x = trans_point[0]
            # point.y = trans_point[1]

            t_point = Point32()
            t_point.x = trans_point[0]
            t_point.y = trans_point[1]
            temp_poly.polygon.points.append(t_point)

        final_polys.polygons.append(temp_poly)
    final_polys.header.frame_id = "odom"
    return final_polys