filter.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 #--------Include modules---------------
4 from copy import copy
5 import rospy
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
10 import tf
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
15 
16 # Subscribers' callbacks------------------------------
17 mapData=OccupancyGrid()
18 frontiers=[]
19 globalmaps=[]
20 def callBack(data,args):
21  global frontiers,min_distance
22  transformedPoint=args[0].transformPoint(args[1],data)
23  x=[array([transformedPoint.point.x,transformedPoint.point.y])]
24  if len(frontiers)>0:
25  frontiers=vstack((frontiers,x))
26  else:
27  frontiers=x
28 
29 
30 def mapCallBack(data):
31  global mapData
32  mapData=data
33 
34 def globalMap(data):
35  global global1,globalmaps,litraIndx,namespace_init_count,n_robots
36  global1=data
37  if n_robots>1:
38  indx=int(data._connection_header['topic'][litraIndx])-namespace_init_count
39  elif n_robots==1:
40  indx=0
41  globalmaps[indx]=data
42 
43 # Node----------------------------------------------
44 def node():
45  global frontiers,mapData,global1,global2,global3,globalmaps,litraIndx,n_robots,namespace_init_count
46  rospy.init_node('filter', anonymous=False)
47 
48  # fetching all parameters
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) #this can be smaller than the laser scanner range, >> smaller >>less computation time>> too small is not good, info gain won't be accurate
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)
59 #-------------------------------------------
60  rospy.Subscriber(map_topic, OccupancyGrid, mapCallBack)
61 
62 
63 #---------------------------------------------------------------------------------------------------------------
64 
65 
66  for i in range(0,n_robots):
67  globalmaps.append(OccupancyGrid())
68 
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)
74 #wait if map is not received yet
75  while (len(mapData.data)<1):
76  pass
77 #wait if any of robots' global costmap map is not received yet
78  for i in range(0,n_robots):
79  while (len(globalmaps[i].data)<1):
80  pass
81 
82  global_frame="/"+mapData.header.frame_id
83 
84 
85  tfLisn=tf.TransformListener()
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))
91 
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)
96 
97  rospy.loginfo("the map and global costmaps are received")
98 
99 
100  # wait if no frontier is received yet
101  while len(frontiers)<1:
102  pass
103 
104 
105  points=Marker()
106  points_clust=Marker()
107 #Set the frame ID and timestamp. See the TF tutorials for information on these.
108  points.header.frame_id= mapData.header.frame_id
109  points.header.stamp= rospy.Time.now()
110 
111  points.ns= "markers2"
112  points.id = 0
113 
114  points.type = Marker.POINTS
115 
116 #Set the marker action for latched frontiers. Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)
117  points.action = Marker.ADD;
118 
119  points.pose.orientation.w = 1.0
120 
121  points.scale.x=0.2
122  points.scale.y=0.2
123 
124  points.color.r = 255.0/255.0
125  points.color.g = 255.0/255.0
126  points.color.b = 0.0/255.0
127 
128  points.color.a=1;
129  points.lifetime = rospy.Duration();
130 
131  p=Point()
132 
133  p.z = 0;
134 
135  pp=[]
136  pl=[]
137 
138  points_clust.header.frame_id= mapData.header.frame_id
139  points_clust.header.stamp= rospy.Time.now()
140 
141  points_clust.ns= "markers3"
142  points_clust.id = 4
143 
144  points_clust.type = Marker.POINTS
145 
146 #Set the marker action for centroids. Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)
147  points_clust.action = Marker.ADD;
148 
149  points_clust.pose.orientation.w = 1.0;
150 
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
156 
157  points_clust.color.a=1;
158  points_clust.lifetime = rospy.Duration();
159 
160 
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
165 
166  arraypoints=PointArray()
167  tempPoint=Point()
168  tempPoint.z=0.0
169 #-------------------------------------------------------------------------
170 #--------------------- Main Loop -------------------------------
171 #-------------------------------------------------------------------------
172  while not rospy.is_shutdown():
173 #-------------------------------------------------------------------------
174 #Clustering frontier points
175  centroids=[]
176  front=copy(frontiers)
177  if len(front)>1:
178  ms = MeanShift(bandwidth=0.3)
179  ms.fit(front)
180  centroids= ms.cluster_centers_ #centroids array is the centers of each cluster
181 
182  #if there is only one frontier no need for clustering, i.e. centroids=frontiers
183  if len(front)==1:
184  centroids=front
185  frontiers=copy(centroids)
186 #-------------------------------------------------------------------------
187 #clearing old frontiers
188 
189  z=0
190  while z<len(centroids):
191  cond=False
192  temppoint.point.x=centroids[z][0]
193  temppoint.point.y=centroids[z][1]
194 
195  for i in range(0,n_robots):
196 
197 
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)
203  z=z-1
204  z+=1
205 #-------------------------------------------------------------------------
206 #publishing
207  arraypoints.points=[]
208  for i in centroids:
209  tempPoint.x=i[0]
210  tempPoint.y=i[1]
211  arraypoints.points.append(copy(tempPoint))
212  filterpub.publish(arraypoints)
213  pp=[]
214  for q in range(0,len(frontiers)):
215  p.x=frontiers[q][0]
216  p.y=frontiers[q][1]
217  pp.append(copy(p))
218  points.points=pp
219  pp=[]
220  for q in range(0,len(centroids)):
221  p.x=centroids[q][0]
222  p.y=centroids[q][1]
223  pp.append(copy(p))
224  points_clust.points=pp
225  pub.publish(points)
226  pub2.publish(points_clust)
227  rate.sleep()
228 #-------------------------------------------------------------------------
229 
230 if __name__ == '__main__':
231  try:
232  node()
233  except rospy.ROSInterruptException:
234  pass
235 
236 
237 
238 
filter.callBack
def callBack(data, args)
Definition: filter.py:20
gridValue
int gridValue(nav_msgs::OccupancyGrid &, std::vector< float >)
Definition: functions.cpp:77
filter.globalMap
def globalMap(data)
Definition: filter.py:34
functions.informationGain
def informationGain(mapData, point, r)
Definition: functions.py:93
filter.node
def node()
Definition: filter.py:44
filter.mapCallBack
def mapCallBack(data)
Definition: filter.py:30

rrt_exploration
Author(s): Hassan Umari
autogenerated on Mon Jun 10 2019 14:57:44