6 from visualization_msgs.msg
import Marker
7 from geometry_msgs.msg
import Point
8 from nav_msgs.msg
import OccupancyGrid
9 from geometry_msgs.msg
import PointStamped
11 from numpy
import array,vstack,delete
12 from functions
import gridValue,informationGain
13 from sklearn.cluster
import MeanShift
14 from rrt_exploration.msg
import PointArray
17 mapData=OccupancyGrid()
21 global frontiers,min_distance
22 transformedPoint=args[0].transformPoint(args[1],data)
23 x=[array([transformedPoint.point.x,transformedPoint.point.y])]
25 frontiers=vstack((frontiers,x))
35 global global1,globalmaps,litraIndx,namespace_init_count,n_robots
38 indx=int(data._connection_header[
'topic'][litraIndx])-namespace_init_count
45 global frontiers,mapData,global1,global2,global3,globalmaps,litraIndx,n_robots,namespace_init_count
46 rospy.init_node(
'filter', anonymous=
False)
49 map_topic= rospy.get_param(
'~map_topic',
'/map')
50 threshold= rospy.get_param(
'~costmap_clearing_threshold',70)
51 info_radius= rospy.get_param(
'~info_radius',1.0)
52 goals_topic= rospy.get_param(
'~goals_topic',
'/detected_points')
53 n_robots = rospy.get_param(
'~n_robots',1)
54 namespace = rospy.get_param(
'~namespace',
'')
55 namespace_init_count = rospy.get_param(
'namespace_init_count',1)
56 rateHz = rospy.get_param(
'~rate',100)
57 litraIndx=len(namespace)
58 rate = rospy.Rate(rateHz)
60 rospy.Subscriber(map_topic, OccupancyGrid, mapCallBack)
66 for i
in range(0,n_robots):
67 globalmaps.append(OccupancyGrid())
69 if len(namespace) > 0:
70 for i
in range(0,n_robots):
71 rospy.Subscriber(namespace+str(i+namespace_init_count)+
'/move_base_node/global_costmap/costmap', OccupancyGrid, globalMap)
72 elif len(namespace)==0:
73 rospy.Subscriber(
'/move_base_node/global_costmap/costmap', OccupancyGrid, globalMap)
75 while (len(mapData.data)<1):
78 for i
in range(0,n_robots):
79 while (len(globalmaps[i].data)<1):
82 global_frame=
"/"+mapData.header.frame_id
86 if len(namespace) > 0:
87 for i
in range(0,n_robots):
88 tfLisn.waitForTransform(global_frame[1:], namespace+str(i+namespace_init_count)+
'/base_link', rospy.Time(0),rospy.Duration(10.0))
89 elif len(namespace)==0:
90 tfLisn.waitForTransform(global_frame[1:],
'/base_link', rospy.Time(0),rospy.Duration(10.0))
92 rospy.Subscriber(goals_topic, PointStamped, callback=callBack,callback_args=[tfLisn,global_frame[1:]])
93 pub = rospy.Publisher(
'frontiers', Marker, queue_size=10)
94 pub2 = rospy.Publisher(
'centroids', Marker, queue_size=10)
95 filterpub = rospy.Publisher(
'filtered_points', PointArray, queue_size=10)
97 rospy.loginfo(
"the map and global costmaps are received")
101 while len(frontiers)<1:
106 points_clust=Marker()
108 points.header.frame_id= mapData.header.frame_id
109 points.header.stamp= rospy.Time.now()
111 points.ns=
"markers2" 114 points.type = Marker.POINTS
117 points.action = Marker.ADD;
119 points.pose.orientation.w = 1.0
124 points.color.r = 255.0/255.0
125 points.color.g = 255.0/255.0
126 points.color.b = 0.0/255.0
129 points.lifetime = rospy.Duration();
138 points_clust.header.frame_id= mapData.header.frame_id
139 points_clust.header.stamp= rospy.Time.now()
141 points_clust.ns=
"markers3" 144 points_clust.type = Marker.POINTS
147 points_clust.action = Marker.ADD;
149 points_clust.pose.orientation.w = 1.0;
151 points_clust.scale.x=0.2;
152 points_clust.scale.y=0.2;
153 points_clust.color.r = 0.0/255.0
154 points_clust.color.g = 255.0/255.0
155 points_clust.color.b = 0.0/255.0
157 points_clust.color.a=1;
158 points_clust.lifetime = rospy.Duration();
161 temppoint=PointStamped()
162 temppoint.header.frame_id= mapData.header.frame_id
163 temppoint.header.stamp=rospy.Time(0)
164 temppoint.point.z=0.0
166 arraypoints=PointArray()
172 while not rospy.is_shutdown():
176 front=copy(frontiers)
178 ms = MeanShift(bandwidth=0.3)
180 centroids= ms.cluster_centers_
185 frontiers=copy(centroids)
190 while z<len(centroids):
192 temppoint.point.x=centroids[z][0]
193 temppoint.point.y=centroids[z][1]
195 for i
in range(0,n_robots):
198 transformedPoint=tfLisn.transformPoint(globalmaps[i].header.frame_id,temppoint)
199 x=array([transformedPoint.point.x,transformedPoint.point.y])
200 cond=(
gridValue(globalmaps[i],x)>threshold)
or cond
201 if (cond
or (
informationGain(mapData,[centroids[z][0],centroids[z][1]],info_radius*0.5))<0.2):
202 centroids=delete(centroids, (z), axis=0)
207 arraypoints.points=[]
211 arraypoints.points.append(copy(tempPoint))
212 filterpub.publish(arraypoints)
214 for q
in range(0,len(frontiers)):
220 for q
in range(0,len(centroids)):
224 points_clust.points=pp
226 pub2.publish(points_clust)
230 if __name__ ==
'__main__':
233 except rospy.ROSInterruptException:
int gridValue(nav_msgs::OccupancyGrid &, std::vector< float >)
def informationGain(mapData, point, r)