def press(self, widget, event):
"""
This code is executed when you press the mouse button
"""
self.emit("focus", Gtk.DirectionType.TAB_FORWARD)
x = event.x / self.zoom
y = event.y / self.zoom
def start_resize(child):
self.unselect_all()
child.selected = True
child.resizing = True
if child.direction < ANONIMOUS:
control = child.handler.control[opposite(child.direction)]
child.pivot.x = self.grid.nearest(control.x)
child.pivot.y = self.grid.nearest(control.y)
child.pivot = self.guides.nearest(child.pivot)
child.handler.pivot.x = control.x
child.handler.pivot.y = control.y
child.handler.pivot.active = True
if self.pick:
self.unselect_all()
#x, y = self.get_pointer()
target = Point()
#child = self.get_child()
child = self.child
self.add(child)
child.selected = True
target.x = self.grid.nearest(x)
target.y = self.grid.nearest(y)
target = self.guides.nearest(target)
child.x = target.x
child.y = target.y
child.width = 0
child.height = 0
child.direction = SOUTHEAST
child.handler.control[opposite(child.direction)].y = child.y
child.handler.control[opposite(child.direction)].x = child.x
start_resize(child)
#widget.bin_window.set_cursor(Gdk.Cursor.new(Gdk.BOTTOM_RIGHT_CORNER))
widget.get_window().set_cursor(Gdk.Cursor.new(Gdk.CursorType.BOTTOM_RIGHT_CORNER))
self.emit("select", child)
return True
selection = True
def start_move(child, x, y):
child.offset.x = x - child.x
child.offset.y = y - child.y
child.press(x, y)
def select(child):
if not event.get_state() & Gdk.ModifierType.CONTROL_MASK:
self.unselect_all()
child.selected = True
for child in sorted(self.document.pages[0].children, key=lambda child: child.z):
if child.selected:
if child.handler.at_position(x, y):
child.direction = child.handler.get_direction(x, y)
selection = False
start_resize(child)
elif child.at_position(x, y):
#start_move(child, x, y)
start_move(child, x, y)
selection = False
else:
continue
elif child.at_position(x, y):
selection = False
select(child)
start_move(child, x, y)
else:
continue
if selection:
self.selection.x = x
self.selection.y = y
self.selection.width = 0
self.selection.height = 0
self.selection.active = True
#widget.bin_window.set_cursor(Gdk.Cursor.new(Gdk.CursorType.CROSSHAIR))
widget.get_window().set_cursor(Gdk.Cursor.new(Gdk.CursorType.CROSSHAIR))
else:
self.stop_cursor_change = True
#self.updated = False
self.update() # XXX
return True
def motion(self, widget, event):
"""
This code is executed when move the mouse pointer
"""
self.statics.motion += 1
#self.consume(Gdk.MOTION_NOTIFY, event.get_state())
self.disconnect(self.motion_id)
if self.horizontal_ruler:
self.horizontal_ruler.motion(self.horizontal_ruler, event, True)
if self.vertical_ruler:
self.vertical_ruler.motion(self.vertical_ruler, event, True)
x = event.x / self.zoom
y = event.y / self.zoom
if not self.stop_cursor_change:
def get_direction_for_child_at_position(x, y, children):
for child in children:
if child.selected and child.handler.at_position(x, y):
direction = child.handler.get_direction(x, y)
#widget.bin_window.set_cursor(child.get_cursor(direction))
widget.get_window().set_cursor(child.get_cursor(direction))
return direction
return NONE
direction = get_direction_for_child_at_position(x, y, self.document.pages[0].children)
if direction == NONE:
if self.pick:
#widget.bin_window.set_cursor(Gdk.Cursor.new(Gdk.PENCIL))
widget.get_window().set_cursor(Gdk.Cursor.new(Gdk.CursorType.PENCIL))
else:
#widget.bin_window.set_cursor(Gdk.Cursor.new(Gdk.CursorType.ARROW))
widget.get_window().set_cursor(Gdk.Cursor.new(Gdk.CursorType.ARROW))
if self.selection.active:
self.selection.width = x - self.selection.x
self.selection.height = y - self.selection.y
self.updated = False
self.update() # XXX
elif event.get_state() & Gdk.ModifierType.BUTTON1_MASK:
for child in self.document.pages[0].children: # TODO
if child.selected:
target = Point()
if child.resizing:
target.x = self.grid.nearest(x)
target.y = self.grid.nearest(y)
target = self.guides.nearest(target)
if child.direction < ANONIMOUS:
child.resize(target.x, target.y)
else:
child.transform(target.x, target.y)
else:
#widget.bin_window.set_cursor(Gdk.Cursor.new(Gdk.FLEUR))
widget.get_window().set_cursor(Gdk.Cursor.new(Gdk.CursorType.FLEUR))
target.x = self.grid.nearest(x - child.offset.x)
target.y = self.grid.nearest(y - child.offset.y)
target = self.guides.nearest(target)
# XXX-TEST
if self.is_testing:
def magnetize(target, children):
for child in children:
if not child.selected and child.magnetos.is_magnetized(target):
return child.magnetos.get_magnetized(target)
return target
target = magnetize(target, self.document.pages[0].children)
# XXX-TEST
child.move(target.x, target.y)
self.emit("edit-child", child)
self.update()
self.motion_id = self.connect("motion-notify-event", self.motion)
return True
def press(self, widget, event):
"""
This code is executed when you press the mouse button
"""
self.emit("focus", gtk.DIR_TAB_FORWARD)
x = event.x / self.zoom
y = event.y / self.zoom
def start_resize(child):
self.unselect_all()
child.selected = True
child.resizing = True
if child.direction < ANONIMOUS:
control = child.handler.control[opposite(child.direction)]
child.pivot.x = self.grid.nearest(control.x)
child.pivot.y = self.grid.nearest(control.y)
child.pivot = self.guides.nearest(child.pivot)
child.handler.pivot.x = control.x
child.handler.pivot.y = control.y
child.handler.pivot.active = True
if self.pick:
self.unselect_all()
#x, y = self.get_pointer()
target = Point()
child = self.child
self.add(child)
child.selected = True
target.x = self.grid.nearest(x)
target.y = self.grid.nearest(y)
target = self.guides.nearest(target)
child.x = target.x
child.y = target.y
child.width = 0
child.height = 0
child.direction = SOUTHEAST
child.handler.control[opposite(child.direction)].y = child.y
child.handler.control[opposite(child.direction)].x = child.x
start_resize(child)
widget.bin_window.set_cursor(gtk.gdk.Cursor(gtk.gdk.BOTTOM_RIGHT_CORNER))
self.emit("select", child)
return True
selection = True
def start_move(child, x, y):
child.offset.x = x - child.x
child.offset.y = y - child.y
child.press(x, y)
def select(child):
if not event.state & gtk.gdk.CONTROL_MASK:
self.unselect_all()
child.selected = True
for child in sorted(self.document.pages[0].children, key=lambda child: child.z):
if child.selected:
if child.handler.at_position(x, y):
child.direction = child.handler.get_direction(x, y)
selection = False
start_resize(child)
elif child.at_position(x, y):
#start_move(child, x, y)
start_move(child, x, y)
selection = False
else:
continue
elif child.at_position(x, y):
selection = False
select(child)
start_move(child, x, y)
else:
continue
if selection:
self.selection.x = x
self.selection.y = y
self.selection.width = 0
self.selection.height = 0
self.selection.active = True
widget.bin_window.set_cursor(gtk.gdk.Cursor(gtk.gdk.CROSSHAIR))
else:
self.stop_cursor_change = True
#self.updated = False
self.update() # XXX
return True
def setinput(self):
pressed = pygame.key.get_pressed()
view = self.perception.look()
self.vehicle = self.buildVehicle(view)
# Mission
# if not self.vehicle.mission.exists():
# self.setVehicleMission()
# mission_msg = MissionMsg()
# for wayp in self.vehicle.mission.waypoints:
# wpm = WaypointMsg()
# wpm.x = wayp.x
# wpm.y = wayp.y
# wpm.type = wayp.type
# wpm.speed_limit = wayp.speed_limit
# mission_msg.waypoints.append(wpm)
# self.pub_mission.publish(mission_msg)
# Pose
pose_msg = PoseMsg()
pose_msg.x = self.vehicle.pose.x
pose_msg.y = self.map.transform_y_back(self.vehicle.pose.y)
pose_msg.heading = -self.vehicle.pose.heading
pose_msg.speed = self.vehicle.pose.speed
self.pub_pose.publish(pose_msg)
# Lanes
lanes_msg = LanesMsg()
right = Point()
right_ahead = Point()
left = Point()
left_ahead = Point()
if self.vehicle.lanes.are_visible():
right.x = self.vehicle.lanes.right.x
right.y = self.map.transform_y_back(self.vehicle.lanes.right.y)
left.x = self.vehicle.lanes.left.x
left.y = self.map.transform_y_back(self.vehicle.lanes.left.y)
right_ahead.x = self.vehicle.lanes.right_ahead.x
right_ahead.y = self.map.transform_y_back(
self.vehicle.lanes.right_ahead.y)
left_ahead.x = self.vehicle.lanes.left_ahead.x
left_ahead.y = self.map.transform_y_back(
self.vehicle.lanes.left_ahead.y)
lanes_msg.right = right
lanes_msg.left = left
lanes_msg.right_ahead = right_ahead
lanes_msg.left_ahead = left_ahead
lanes_msg.right.z = 0
lanes_msg.left.z = 0
lanes_msg.right_ahead.z = 0
lanes_msg.left_ahead.z = 0
self.pub_lanes.publish(lanes_msg)
if not pressed[K_q]:
# autonomous player
# build modified target (for path tracker)
target = Target()
target.speed_limit = self.vehicle.target.speed_limit
target.start.x = self.vehicle.target.start.x
target.start.y = self.map.transform_y_back(
self.vehicle.target.start.y)
target.start.heading = self.vehicle.target.start.heading
target.start.speed = self.vehicle.target.start.speed
target.end.x = self.vehicle.target.end.x
target.end.y = self.map.transform_y_back(self.vehicle.target.end.y)
target.end.heading = self.vehicle.target.end.heading
target.end.speed = self.vehicle.target.end.speed
# set local reference frame for trajectory
# all angles constrained in [-pi/pi]
wp_traversed = self.has_changed(target)
if not self.frame or wp_traversed:
# Harrit has y as car longitudinal axis
car_heading = math.atan2(math.sin(self.car.pose + math.pi / 2),
math.cos(self.car.pose + math.pi / 2))
self.frame = Frame(target.start.x, target.start.y, car_heading)
print "new frame:", target.start.x, target.start.y, target.end.x, target.end.y, self.car.center[
0], self.car.center[1], car_heading * 180 / math.pi
# convert to local frame
current_wp_x, current_wp_y = self.frame.global2local(
target.start.x, target.start.y)
next_wp_x, next_wp_y = self.frame.global2local(
target.end.x, target.end.y)
car_x, car_y = self.frame.global2local(self.car.center[0],
self.car.center[1])
if (current_wp_x, current_wp_y) != (
next_wp_x,
next_wp_y) and target.start.x and target.start.y:
target_heading = -target.end.heading - self.frame.angle # Compensate for clockwise angle
target_heading = math.atan2(
math.sin(target_heading),
math.cos(target_heading)) # Between [-pi, pi]
target_heading = target_heading if target_heading > 0 else 2 * pi - target_heading # Between [0, 2*pi]
inst_heading = -self.car.pose - self.frame.angle # Compensate for clockwise angle
inst_heading = math.atan2(
math.sin(inst_heading),
math.cos(inst_heading)) # Between [-pi, pi]
inst_heading = inst_heading if inst_heading > 0 else 2 * pi - inst_heading # Between [0, 2*pi]
print "cwx: ", current_wp_x, "cwy:", current_wp_y, "nwx:", next_wp_x, "nwy:", next_wp_y, 5, 5, \
"cx:", car_x, "cy:", car_y, "s:", self.car.speed, "th: {0:.2f}".format(
target_heading*180/math.pi), "ih: {0:.2f}".format(
inst_heading*180/math.pi), "trv:", wp_traversed
trajectory = self.planner.trajectory_planner(
current_wp_x=current_wp_x,
current_wp_y=current_wp_y,
next_wp_x=next_wp_x,
next_wp_y=next_wp_y,
current_wp_v=target.start.speed,
next_wp_v=target.end.speed,
# current_wp_v=5, next_wp_v=5,
current_x=car_x,
current_y=car_y,
inst_v=self.car.speed,
heading=target_heading,
inst_heading=inst_heading,
wp_traversed=wp_traversed)
print trajectory
# Trajectory is empty, should recompute
if len(trajectory[0]) < 2:
print "Empty trajectory, mission failed!"
self.tracker.setTarget(target)
return
global_traj = [[0 for x in range(len(trajectory[0]))]
for y in range(2)]
for i in xrange(0, len(trajectory[0])):
(tx, ty) = self.frame.local2global(trajectory[0][i],
trajectory[1][i])
global_traj[0][i] = tx
global_traj[1][i] = ty
# if len(global_traj[0]) > 0:
# idx = int(min(len(global_traj[0]) - 1, max(10 * self.car.speed, 20)))
# target.end.x = global_traj[0][idx]
# target.end.y = global_traj[1][idx]
input = self.tracker.getInput(target, global_traj)
self.car.acc = input['acc']
self.car.steer = input['steer']
# Lateral error too large, should recompute
if input['error']:
print "Lateral error too large, mission failed!"
return
else:
# human player
if pressed[K_LEFT]:
self.car.steer += -radians(4)
elif pressed[K_RIGHT]:
self.car.steer += radians(4)
else:
self.car.steer = 0
if pressed[K_UP]:
self.car.acc += 1
elif pressed[K_DOWN]:
self.car.acc += -1
else:
self.car.acc = 0
def motion(self, widget, event):
"""
This code is executed when move the mouse pointer
"""
self.statics.motion += 1
#self.consume(gtk.gdk.MOTION_NOTIFY, event.state)
self.disconnect(self.motion_id)
if self.horizontal_ruler:
self.horizontal_ruler.motion(self.horizontal_ruler, event, True)
if self.vertical_ruler:
self.vertical_ruler.motion(self.vertical_ruler, event, True)
x = event.x / self.zoom
y = event.y / self.zoom
if not self.stop_cursor_change:
def get_direction_for_child_at_position(x, y, children):
for child in children:
if child.selected and child.handler.at_position(x, y):
direction = child.handler.get_direction(x, y)
widget.bin_window.set_cursor(child.get_cursor(direction))
return direction
return NONE
direction = get_direction_for_child_at_position(x, y, self.document.pages[0].children)
if direction == NONE:
if self.pick:
widget.bin_window.set_cursor(gtk.gdk.Cursor(gtk.gdk.PENCIL))
else:
widget.bin_window.set_cursor(gtk.gdk.Cursor(gtk.gdk.ARROW))
if self.selection.active:
self.selection.width = x - self.selection.x
self.selection.height = y - self.selection.y
self.updated = False
self.update() # XXX
elif event.state & gtk.gdk.BUTTON1_MASK:
for child in self.document.pages[0].children: # TODO
if child.selected:
target = Point()
if child.resizing:
target.x = self.grid.nearest(x)
target.y = self.grid.nearest(y)
target = self.guides.nearest(target)
if child.direction < ANONIMOUS:
child.resize(target.x, target.y)
else:
child.transform(target.x, target.y)
else:
widget.bin_window.set_cursor(gtk.gdk.Cursor(gtk.gdk.FLEUR))
target.x = self.grid.nearest(x - child.offset.x)
target.y = self.grid.nearest(y - child.offset.y)
target = self.guides.nearest(target)
# XXX-TEST
if self.is_testing:
def magnetize(target, children):
for child in children:
if not child.selected and child.magnetos.is_magnetized(target):
return child.magnetos.get_magnetized(target)
return target
target = magnetize(target, self.document.pages[0].children)
# XXX-TEST
child.move(target.x, target.y)
self.emit("edit-child", child)
self.update()
self.motion_id = self.connect("motion-notify-event", self.motion)
return True
