Go to the documentation of this file.00001
00002
00003 import roslib; roslib.load_manifest('pacman_controller')
00004 from visualization_msgs.msg import Marker
00005 from visualization_msgs.msg import MarkerArray
00006 from geometry_msgs.msg import PoseStamped,Point
00007 from std_msgs.msg import String
00008 import commands
00009
00010 import rospy
00011 import math
00012
00013 import pacman_controller.game_engine as GE
00014 THRESHOLD = 0.3
00015
00016 def dist(a, b):
00017 return math.sqrt(math.pow(a['x'] - b['x'], 2) + math.pow(a['y'] - b['y'], 2))
00018
00019 class createMap():
00020 def __init__(self):
00021 topic = 'visualization_markers'
00022 self.publisher = rospy.Publisher(topic, MarkerArray)
00023 self.sub_click = rospy.Subscriber("click",PoseStamped, self.click_callback)
00024 self.sub_unclick = rospy.Subscriber("unclick",PoseStamped, self.unclick_callback)
00025 self.sub_command = rospy.Subscriber("/commands_robot", String, self.commands_robot_callback)
00026
00027 self.points = []
00028 self.edges = []
00029 self.cur = -1
00030 self.count = 0
00031
00032 if rospy.has_param("vertices"):
00033 gameEngine = GE.GameEngine()
00034 self.points = gameEngine.points
00035 self.edges = gameEngine.edges
00036 self.count = len(self.points)
00037
00038 def commands_robot_callback(self,msg):
00039 if msg.data == "Save Map":
00040 self.saveMap()
00041
00042 def saveMap(self):
00043 direct = commands.getoutput('rospack find pacman_controller')
00044 yamlWrite = open(direct + '/map_created.yaml','w')
00045
00046 yamlWrite.write("vertices:\n")
00047 for point in self.points:
00048 yamlWrite.write(" point_%d:\n"%point['id'])
00049 yamlWrite.write(" id: %d\n"%point['id'])
00050 yamlWrite.write(" x: %f\n"%point['x'])
00051 yamlWrite.write(" y: %f\n"%point['y'])
00052
00053 yamlWrite.write("edges:\n")
00054 for i in range(len(self.edges)):
00055 yamlWrite.write(" edge_%d:\n"%i)
00056 yamlWrite.write(" a: %d\n"%self.edges[i]['a'])
00057 yamlWrite.write(" b: %d\n"%self.edges[i]['b'])
00058
00059 yamlWrite.close()
00060
00061
00062 def unclick_callback(self,msg):
00063 point={}
00064 point['x'] = msg.pose.position.x
00065 point['y'] = msg.pose.position.y
00066
00067 index = self.get_point(point)
00068 if (not index == -1):
00069 del self.points[index]
00070 self.delete_edges(index)
00071
00072 for i in range(len(self.points)):
00073 self.points[i]['id'] = i
00074
00075 self.count -= 1
00076 print str(self.points)
00077 print str(self.edges)
00078
00079
00080 def delete_edges(self,index):
00081 for edge in self.edges:
00082 if index in edge:
00083 self.edges.remove(edge)
00084 continue
00085 if edge['a']>index:
00086 edge['a'] -= 1
00087 if edge['b']>index:
00088 edge['b'] -= 1
00089
00090
00091 def click_callback(self,msg):
00092 point={}
00093 point['x'] = msg.pose.position.x
00094 point['y'] = msg.pose.position.y
00095
00096 index = self.get_point(point)
00097 if not (index == -1):
00098 if self.cur == index:
00099 self.cur = -1
00100 else:
00101 if not (self.cur == -1):
00102 self.add_neighbor(self.cur, index)
00103 self.cur = index
00104 else:
00105 point['id'] = self.count
00106 self.points.append(point)
00107 if not self.cur == -1:
00108 self.add_neighbor(self.cur, self.count)
00109 self.cur = self.count;
00110 self.count +=1
00111 print str(self.points)
00112 print str(self.edges)
00113
00114 def add_neighbor(self, a, b):
00115 edgeA = {'a':a, 'b':b}
00116 edgeB = {'a':b, 'b':a}
00117 if (edgeA in self.edges or edgeB in self.edges):
00118 return
00119 else:
00120 self.edges.append(edgeA)
00121
00122 def get_point(self, position):
00123 for point in self.points:
00124 if (abs(dist(point,position)) < THRESHOLD):
00125 return point['id']
00126 return -1;
00127
00128 def get_dir(self, point_a, point_b):
00129 result_x = point_b['x'] - point_a['x']
00130 result_y = point_b['y'] - point_a['y']
00131 dis = dist(point_a,point_b)
00132 return [result_x / dis, result_y / dis]
00133
00134 def publish_markers(self):
00135
00136 markerArray = MarkerArray()
00137 marker = Marker()
00138 marker.header.frame_id = "/map"
00139 marker.header.stamp = rospy.get_rostime()
00140 marker.type = marker.SPHERE_LIST
00141 marker.action = marker.ADD
00142 marker.scale.x = 0.2
00143 marker.scale.y = 0.2
00144 marker.scale.z = 0.2
00145 marker.color.a = 1.0
00146 marker.color.r = 0.0
00147 marker.color.g = 1.0
00148 marker.color.b = 0.0
00149 marker.pose.orientation.w = 1.0
00150 marker.pose.position.x = 0.0
00151 marker.pose.position.y = 0.0
00152 marker.pose.position.z = 0.0
00153 marker.id = 0
00154 for p in self.points:
00155 point = Point()
00156 point.x = p['x']
00157 point.y = p['y']
00158 if p['id'] == self.cur:
00159 continue
00160 marker.points.append(point)
00161 markerArray.markers.append(marker)
00162
00163
00164
00165 for p in self.points:
00166 marker = Marker()
00167 marker.header.frame_id = "/map"
00168 marker.header.stamp = rospy.get_rostime()
00169 marker.type = marker.TEXT_VIEW_FACING
00170 marker.action = marker.ADD
00171 marker.scale.x = 0.2
00172 marker.scale.y = 0.2
00173 marker.scale.z = 0.2
00174 marker.color.a = 1.0
00175 marker.color.r = 1.0
00176 marker.color.g = 0.0
00177 marker.color.b = 0.0
00178 marker.pose.orientation.w = 1.0
00179 marker.pose.position.x = p['x']
00180 marker.pose.position.y = p['y']
00181 marker.pose.position.z = 0.0
00182 marker.id = len(markerArray.markers)
00183 marker.text = 'Node: ' + str(p['id'])
00184 markerArray.markers.append(marker)
00185
00186
00187
00188 marker = Marker()
00189 marker.header.frame_id = "/map"
00190 marker.header.stamp = rospy.get_rostime()
00191 marker.type = marker.SPHERE_LIST
00192 marker.action = marker.ADD
00193 marker.scale.x = 0.05
00194 marker.scale.y = 0.05
00195 marker.scale.z = 0.05
00196 marker.color.a = 1.0
00197 marker.color.r = 0.0
00198 marker.color.g = 1.0
00199 marker.color.b = 0.0
00200 marker.pose.orientation.w = 1.0
00201 marker.pose.position.x = 0.0
00202 marker.pose.position.y = 0.0
00203 marker.pose.position.z = 0.0
00204 marker.id = len(markerArray.markers)
00205 for edge in self.edges:
00206
00207 point_a = self.points[edge['a']]
00208 point_b = self.points[edge['b']]
00209
00210 cur_point = {}
00211 cur_point['x'] = point_a['x']
00212 cur_point['y'] = point_a['y']
00213 direction = self.get_dir(point_a, point_b)
00214 while (dist(cur_point, point_b) > 0.2):
00215 point = Point()
00216 point.x = cur_point['x'] + 0.2 * direction[0]
00217 point.y = cur_point['y'] + 0.2 * direction[1]
00218 cur_point['x'] = point.x
00219 cur_point['y'] = point.y
00220 marker.points.append(point)
00221
00222 markerArray.markers.append(marker)
00223
00224 if not (self.cur == -1):
00225
00226 marker = Marker()
00227 marker.header.frame_id = "/map"
00228 marker.header.stamp = rospy.get_rostime()
00229
00230 marker.type = marker.SPHERE
00231 marker.action = marker.ADD
00232 marker.scale.x = 0.2
00233 marker.scale.y = 0.2
00234 marker.scale.z = 0.2
00235 marker.color.a = 1.0
00236 marker.color.r = 1.0
00237 marker.color.g = 1.0
00238 marker.color.b = 0.0
00239 marker.pose.orientation.w = 1.0
00240 marker.pose.position.x = self.points[self.cur]['x']
00241 marker.pose.position.y = self.points[self.cur]['y']
00242 marker.pose.position.z = 0.0
00243 marker.id = len(markerArray.markers)
00244 markerArray.markers.append(marker)
00245 else:
00246 marker = Marker()
00247 marker.header.frame_id = "/map"
00248 marker.header.stamp = rospy.get_rostime()
00249
00250 marker.type = marker.SPHERE
00251 marker.action = marker.DELETE
00252 marker.id = len(markerArray.markers)
00253 markerArray.markers.append(marker)
00254
00255
00256 self.publisher.publish(markerArray)
00257
00258
00259 if __name__ == '__main__':
00260 rospy.init_node('createMap')
00261 createMapApp = createMap()
00262 while not rospy.is_shutdown():
00263 createMapApp.publish_markers()
00264 rospy.sleep(0.1)
00265
