示例#1
0
def node():
	global frontiers,mapData,global1,global2,global3,globalmaps
	rospy.init_node('filter', anonymous=False)
	
	# fetching all parameters
	map_topic= rospy.get_param('~map_topic','/robot_1/map')
	threshold= rospy.get_param('~costmap_clearing_threshold',70)
	info_radius= rospy.get_param('~info_radius',1.0)					#this can be smaller than the laser scanner range, >> smaller >>less computation time>> too small is not good, info gain won't be accurate
	goals_topic= rospy.get_param('~goals_topic','/detected_points')	
	n_robots = rospy.get_param('~n_robots',1)
	rateHz = rospy.get_param('~rate',100)
	
	rate = rospy.Rate(rateHz)
#-------------------------------------------
	rospy.Subscriber(map_topic, OccupancyGrid, mapCallBack)
	

#---------------------------------------------------------------------------------------------------------------
	

	for i in range(0,n_robots):
 		 globalmaps.append(OccupancyGrid()) 
 		 
 	for i in range(0,n_robots):
		rospy.Subscriber('/robot_'+str(i+1)+'/move_base_node/global_costmap/costmap', OccupancyGrid, globalMap) 
		
#wait if map is not received yet
	while (len(mapData.data)<1):
		pass
#wait if any of robots' global costmap map is not received yet
	for i in range(0,n_robots):
 		 while (len(globalmaps[i].data)<1):
 		 	pass
	
	global_frame="/"+mapData.header.frame_id


	tfLisn=tf.TransformListener()
	for i in range(0,n_robots):
		tfLisn.waitForTransform(global_frame[1:], 'robot_'+str(i+1)+'/base_link', rospy.Time(0),rospy.Duration(10.0))
	
	rospy.Subscriber(goals_topic, PointStamped, callback=callBack,callback_args=[tfLisn,global_frame[1:]])
	pub = rospy.Publisher('frontiers', Marker, queue_size=10)
	pub2 = rospy.Publisher('centroids', Marker, queue_size=10)
	filterpub = rospy.Publisher('filtered_points', PointArray, queue_size=10)

	rospy.loginfo("the map and global costmaps are received")
	
	
	# wait if no frontier is received yet 
	while len(frontiers)<1:
		pass
	
	
	points=Marker()
	points_clust=Marker()
#Set the frame ID and timestamp.  See the TF tutorials for information on these.
	points.header.frame_id= mapData.header.frame_id
	points.header.stamp= rospy.Time.now()

	points.ns= "markers2"
	points.id = 0
	
	points.type = Marker.POINTS
	
#Set the marker action for latched frontiers.  Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)
	points.action = Marker.ADD;

	points.pose.orientation.w = 1.0

	points.scale.x=0.2
	points.scale.y=0.2 

	points.color.r = 255.0/255.0
	points.color.g = 255.0/255.0
	points.color.b = 0.0/255.0

	points.color.a=1;
	points.lifetime = rospy.Duration();

	p=Point()

	p.z = 0;

	pp=[]
	pl=[]
	
	points_clust.header.frame_id= mapData.header.frame_id
	points_clust.header.stamp= rospy.Time.now()

	points_clust.ns= "markers3"
	points_clust.id = 4

	points_clust.type = Marker.POINTS

#Set the marker action for centroids.  Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)
	points_clust.action = Marker.ADD;

	points_clust.pose.orientation.w = 1.0;

	points_clust.scale.x=0.2;
	points_clust.scale.y=0.2; 
	points_clust.color.r = 0.0/255.0
	points_clust.color.g = 255.0/255.0
	points_clust.color.b = 0.0/255.0

	points_clust.color.a=1;
	points_clust.lifetime = rospy.Duration();

		
	temppoint=PointStamped()
	temppoint.header.frame_id= mapData.header.frame_id
	temppoint.header.stamp=rospy.Time(0)
	temppoint.point.z=0.0
	
	arraypoints=PointArray()
	tempPoint=Point()
	tempPoint.z=0.0
#-------------------------------------------------------------------------
#---------------------     Main   Loop     -------------------------------
#-------------------------------------------------------------------------
	while not rospy.is_shutdown():
#-------------------------------------------------------------------------	
#Clustering frontier points
		centroids=[]
		front=copy(frontiers)
		if len(front)>1:
			ms = MeanShift(bandwidth=0.3)   
			ms.fit(front)
			centroids= ms.cluster_centers_	 #centroids array is the centers of each cluster		

		#if there is only one frontier no need for clustering, i.e. centroids=frontiers
		if len(front)==1:
			centroids=front
		frontiers=copy(centroids)
#-------------------------------------------------------------------------	
#clearing old frontiers  
      
		z=0
		while z<len(centroids):
			cond=False
			temppoint.point.x=centroids[z][0]
			temppoint.point.y=centroids[z][1]
						
			for i in range(0,n_robots):
				
				
				transformedPoint=tfLisn.transformPoint(globalmaps[i].header.frame_id,temppoint)
				x=array([transformedPoint.point.x,transformedPoint.point.y])
				cond=(gridValue(globalmaps[i],x)>threshold) or cond
			if (cond or (informationGain(mapData,[centroids[z][0],centroids[z][1]],info_radius*0.5))<0.2):
				centroids=delete(centroids, (z), axis=0)
				z=z-1
			z+=1
#-------------------------------------------------------------------------
#publishing
		arraypoints.points=[]
		for i in centroids:
			tempPoint.x=i[0]
			tempPoint.y=i[1]
			arraypoints.points.append(copy(tempPoint))
		filterpub.publish(arraypoints)
		pp=[]	
		for q in range(0,len(frontiers)):
			p.x=frontiers[q][0]
			p.y=frontiers[q][1]
			pp.append(copy(p))
		points.points=pp
		pp=[]	
		for q in range(0,len(centroids)):
			p.x=centroids[q][0]
			p.y=centroids[q][1]
			pp.append(copy(p))
		points_clust.points=pp
			
		pub.publish(points)
		pub2.publish(points_clust) 
		rate.sleep()
示例#2
0
def node():
    print('usao u glavni')
    global mapData, global1, global2, global3, globalmaps, current_frontier, all_frontiers, litraIndx, n_robots, namespace_init_count
    rospy.init_node('filter', anonymous=False)
    # fetching all parameters
    map_topic = rospy.get_param('~map_topic', '/map')
    threshold = rospy.get_param('~costmap_clearing_threshold', 50)
    info_radius = rospy.get_param(
        '~info_radius', 1
    )  #this can be smaller than the laser scanner range, >> smaller >>less computation time>> too small is not good, info gain won't be accurate
    goals_topic = rospy.get_param('~goals_topic', '/detected_points')
    transform_topic = rospy.get_param('~transform_topic',
                                      '/transformed_points')
    submap_topic = rospy.get_param('~submap_topic', '/used_submaps')
    n_robots = rospy.get_param('~n_robots', 1)
    namespace_list = rospy.get_param('~namespace_list', ['alpha'])
    namespace_init_count = rospy.get_param('namespace_init_count', 1)
    rateHz = rospy.get_param('~rate', 100)
    litraIndx = len(namespace_list)
    rate = rospy.Rate(rateHz)
    #-------------------------------------------
    rospy.Subscriber(map_topic, OccupancyGrid, mapCallBack)

    #---------------------------------------------------------------------------------------------------------------

    for i in range(0, n_robots):
        globalmaps.append(OccupancyGrid())

    if len(namespace_list) > 0:
        for i in range(0, len(namespace_list)):
            print(namespace_list[i])
            rospy.Subscriber(
                namespace_list[i] + '/move_base/global_costmap/costmap',
                OccupancyGrid, globalMap)
    elif len(namespace_list) == 0:
        rospy.Subscriber('/move_base/global_costmap/costmap', OccupancyGrid,
                         globalMap)
    print('usao u glavni2')
    #wait if map is not received yet
    while (len(mapData.data) < 1):
        pass
    print('usao u glavni3')
    #wait if any of robots' global costmap map is not received yet
    for i in range(0, n_robots):
        while (len(globalmaps[i].data) < 1):
            pass
    global_frame = "/" + mapData.header.frame_id

    print('usao u glavni4')
    tfLisn = tf.TransformListener()
    rospy.sleep(2)
    if len(namespace_list) > 0:
        for i in range(0, len(namespace_list)):
            tfLisn.waitForTransform(global_frame[1:],
                                    namespace_list[i] + '/base_link',
                                    rospy.Time(0), rospy.Duration(10.0))
    elif len(namespace_list) == 0:
        tfLisn.waitForTransform(global_frame[1:], '/base_link', rospy.Time(0),
                                rospy.Duration(10.0))

    rospy.Subscriber(submap_topic, SubmapList, submapCallBack)
    rospy.Subscriber(transform_topic, FrontierTF, transformCallBack)
    #rospy.Subscriber(goals_topic, PointStamped, callback=callBack,callback_args=[tfLisn,global_frame[1:]])

    pub = rospy.Publisher('frontiers', Marker, queue_size=10)
    pub2 = rospy.Publisher('centroids', Marker, queue_size=10)
    filterpub = rospy.Publisher('filtered_points', PointArray, queue_size=10)
    filterpub2 = rospy.Publisher('filtered_struct', FrontierTF, queue_size=10)

    rospy.loginfo("the map and global costmaps are received")

    # wait if no frontier is received yet
    while len(current_frontier) < 1:
        pass

    points = Marker()
    points_clust = Marker()
    #Set the frame ID and timestamp.  See the TF tutorials for information on these.
    points.header.frame_id = mapData.header.frame_id
    points.header.stamp = rospy.Time.now()

    points.ns = "markers2"
    points.id = 0

    points.type = Marker.POINTS
    #Set the marker action for latched frontiers.  Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)
    points.action = Marker.ADD

    points.pose.orientation.w = 1.0

    points.scale.x = 0.2
    points.scale.y = 0.2

    points.color.r = 255.0 / 255.0
    points.color.g = 255.0 / 255.0
    points.color.b = 0.0 / 255.0

    points.color.a = 1
    points.lifetime = rospy.Duration()

    p = Point()

    p.z = 0

    pp = []
    pl = []

    points_clust.header.frame_id = mapData.header.frame_id
    points_clust.header.stamp = rospy.Time.now()

    points_clust.ns = "markers3"
    points_clust.id = 4

    points_clust.type = Marker.POINTS

    #Set the marker action for centroids.  Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)
    points_clust.action = Marker.ADD

    points_clust.pose.orientation.w = 1.0

    points_clust.scale.x = 0.2
    points_clust.scale.y = 0.2
    points_clust.color.r = 0.0 / 255.0
    points_clust.color.g = 255.0 / 255.0
    points_clust.color.b = 0.0 / 255.0
    points_clust.color.a = 1
    points_clust.lifetime = rospy.Duration()

    temppoint = PointStamped()
    temppoint.header.frame_id = mapData.header.frame_id
    temppoint.header.stamp = rospy.Time(0)
    temppoint.point.z = 0.0

    arraypoints = PointArray()
    tempPoint = Point()
    tempPoint.z = 0.0
    #-------------------------------------------------------------------------
    #---------------------     Main   Loop     -------------------------------
    #-------------------------------------------------------------------------
    while not rospy.is_shutdown():
        #-------------------------------------------------------------------------
        #SVE FRONTIERE PONOVNO TRANSFORMIRATI, U SVAKOM PROLAZU

        my_frontiers = copy(current_frontier)
        transform(my_frontiers)

        transform(all_frontiers)
        all_frontiers.extend(my_frontiers)

        #print ('koje dobivam', all_frontiers)
        all_centroids = copy(all_frontiers)
        #print('centroidi su:', all_centroids[0])
        #-------------------------------------------------------------------------
        #clearing old frontiers
        z = 0
        #print ('duljina frontiera',len(all_frontiers))
        for centroid in all_centroids:
            cond = False
            temppoint.point.x = centroid.projected.x
            temppoint.point.y = centroid.projected.y

            for i in range(0, len(namespace_list)):
                transformedPoint = tfLisn.transformPoint(
                    globalmaps[i].header.frame_id, temppoint)
                x = array([transformedPoint.point.x, transformedPoint.point.y])
                cond = (gridValue(globalmaps[i], x) > threshold) or cond
            if (cond or (informationGain(
                    mapData, [centroid.projected.x, centroid.projected.y],
                    info_radius * 0.5)) < 0.3):
                all_centroids = list(delete(all_centroids, (z), axis=0))
                z = z - 1
            #print ('deleted centroid')
            z += 1
        print("resize frontiers from %d to %d" %
              (len(all_frontiers), len(all_centroids)))
        #-------------------------------------------------------------------------

        #------------------------------------------------------------------------
        #publishing

        arraypoints.points = []
        #arraystruct=[]
        cnt = 0

        for centroid in all_centroids:

            tempPoint.x = centroid.projected.x
            tempPoint.y = centroid.projected.y
            arraypoints.points.append(copy(tempPoint))
            #arraystruct.append(copy(centroid))
            cnt += 1

        filterpub.publish(arraypoints)
        #filterpub2.publish(arraystruct)
        print("Published transformed centroids:" + str(cnt))
        #-------------------------------------------------------------------------
        pp = []
        for q in all_centroids:
            p.x = q.projected.x
            p.y = q.projected.y
            pp.append(copy(p))
        points_clust.points = pp

        pub2.publish(points_clust)

        print("delete old frontiers")
        z = 0
        for frontier in all_frontiers:
            cond = False
            temppoint.point.x = frontier.projected.x
            temppoint.point.y = frontier.projected.y

            for i in range(0, len(namespace_list)):
                transformedPoint = tfLisn.transformPoint(
                    globalmaps[i].header.frame_id, temppoint)
                x = array([transformedPoint.point.x, transformedPoint.point.y])
                cond = (gridValue(globalmaps[i], x) > threshold) or cond
            if (cond or (informationGain(
                    mapData, [frontier.projected.x, frontier.projected.y],
                    info_radius * 0.5)) < 0.2):
                all_frontiers = list(delete(all_frontiers, (z), axis=0))
                z = z - 1
            z += 1
        pp = []
        for q in all_frontiers:
            p.x = q.projected.x
            p.y = q.projected.y
            pp.append(copy(p))
        points.points = pp
        pub.publish(points)
        rate.sleep()