def draw_point(self, x, y):
package = Debug_Msgs()
msg = package.msgs.add()
# line 1
msg.type = Debug_Msg.LINE
msg.color = Debug_Msg.WHITE
line = msg.line
line.start.x = x + 50
line.start.y = y + 50
line.end.x = x - 50
line.end.y = y - 50
line.FORWARD = True
line.BACK = True
# line 2
msg = package.msgs.add()
msg.type = Debug_Msg.LINE
msg.color = Debug_Msg.WHITE
line = msg.line
line.start.x = x - 50
line.start.y = y + 50
line.end.x = x + 50
line.end.y = y - 50
line.FORWARD = True
line.BACK = True
self.sock.sendto(package.SerializeToString(), self.debug_address)
def draw_point(self, x, y):
package = Debug_Msgs()
msg = package.msgs.add()
msg.type = Debug_Msg.POINT
msg.color = Debug_Msg.WHITE
point = msg.point
point.x = x
point.y = y
self.sock.sendto(package.SerializeToString(), self.debug_address)
def draw_all(self, path_x, path_y, robot_x, robot_y):
package = Debug_Msgs()
self.draw_grids(package)
self.draw_points(package, path_x, path_y)
#
# self.draw_roadmap(package, sample_x, sample_y, road_map)
self.draw_finalpath(package, path_x, path_y)
self.draw_circle(robot_x, robot_y)
self.sock.sendto(package.SerializeToString(), self.debug_address)
def draw_robot(self, x, y, dir):
package = Debug_Msgs()
msg = package.msgs.add()
msg.type = Debug_Msg.ROBOT
msg.color = Debug_Msg.WHITE
robo = msg.robot
robo.pos.x = x
robo.pos.y = y
robo.dir = dir
self.sock.sendto(package.SerializeToString(), self.debug_address)
def robot_msg(self, x=0, y=0, orientate=0):
package = Debug_Msgs()
msg = package.msgs.add()
msg.type = Debug_Msg.ROBOT
msg.color = Debug_Msg.RED
robot = msg.robot
point = robot.pos
point.x = x
point.y = y
robot.dir = orientate
self.sock.sendto(package.SerializeToString(), self.debug_address)
def draw_line(self, x1, y1, x2, y2):
package = Debug_Msgs()
msg = package.msgs.add()
msg.type = Debug_Msg.LINE
msg.color = Debug_Msg.WHITE
line = msg.line
line.start.x = x1
line.start.y = y1
line.end.x = x2
line.end.y = y2
line.FORWARD = True
line.BACK = True
self.sock.sendto(package.SerializeToString(), self.debug_address)
def draw_circle(self, x, y):
package = Debug_Msgs()
msg = package.msgs.add()
msg.type = Debug_Msg.ARC
msg.color = Debug_Msg.WHITE
arc = msg.arc
arc.rectangle.point1.x = x - self.Circle_Size
arc.rectangle.point1.y = y - self.Circle_Size
arc.rectangle.point2.x = x + self.Circle_Size
arc.rectangle.point2.y = y + self.Circle_Size
arc.start = 0
arc.end = 360
arc.FILL = True
self.sock.sendto(package.SerializeToString(), self.debug_address)
def draw_final_line(self, final_list, T, startpoint):
package = Debug_Msgs()
for i in range(len(final_list) - 1):
msg = package.msgs.add()
msg.type = Debug_Msg.LINE
msg.color = Debug_Msg.GREEN
line = msg.line
line.start.x = final_list[i][0]
line.start.y = final_list[i][1]
line.end.x = final_list[i + 1][0]
line.end.y = final_list[i + 1][1]
line.FORWARD = True
line.BACK = False
#self.Draw_PreorderTraverse(T,startpoint)
self.sock.sendto(package.SerializeToString(), self.debug_address)
def draw_line(self, x_start=0, y_start=0, x_end=0, y_end=0):
package = Debug_Msgs()
msg = package.msgs.add()
msg.type = Debug_Msg.LINE
msg.color = Debug_Msg.RED
line = msg.line
p_start = line.start
p_end = line.end
p_start.x = x_start
p_start.y = y_start
p_end.x = x_end
p_end.y = y_end
line.FORWARD = True
line.BACK = False
self.sock.sendto(package.SerializeToString(), self.debug_address)