def main():
rospy.on_shutdown(offhook)
global angle
global velocity
key = ''
while key != ord('q'):
key = stdscr.getch()
stdscr.refresh()
if key == curses.KEY_LEFT and angle > -100 + increment:
angle -= increment
elif key == curses.KEY_RIGHT and angle < 100 - increment:
angle += increment
elif key == curses.KEY_UP and velocity < 100 + increment:
velocity += increment
elif key == curses.KEY_DOWN and velocity > -100 + increment:
velocity -= increment
stdscr.addstr(0, 0, "angle: ")
stdscr.addstr(0, 7, '%3.2f' % angle)
stdscr.addstr(1, 0, "velocity: ")
stdscr.addstr(1, 10, '%3.2f' % velocity)
message = drive_param()
message.velocity = velocity
message.angle = angle
control_drive_parameters.publish(message)
curses.endwin()
def offhook():
control_drive_parameters = rospy.Publisher(
'vugc1_control_drive_parameters', drive_param, queue_size=10)
message = drive_param()
message.velocity = 0
message.angle = 0
control_drive_parameters.publish(message)
def callback(data):
rospy.on_shutdown(offhook)
# angle_range = math.degrees(data.angle_max - data.angle_min)
angle_range = data.angle_max - data.angle_min
print('angle ranges', angle_range)
forward = 0.5 * angle_range
actual_dists = [0, 0, 0]
l_dist = 0
r_dist = 0
if mode == 'centering':
right = 0.4 * angle_range
left = 0.6 * angle_range
angles = [forward, left, right]
print('angles', angles)
actual_dists = get_range(data, angles)
r_dist = actual_dists[2]
l_dist = actual_dists[1]
else:
angle1 = math.radians(45)
angle2 = math.radians(60)
angles = [forward, angle1, angle2]
actual_dists = get_range(data, angles)
swing = math.radians(angle2 - angle1)
alpha = math.atan2(
(actual_dists[2] * math.cos(swing)) - actual_dists[1],
actual_dists[2] * math.sin(swing))
AB = actual_dists[1] * math.cos(alpha)
AC = 1
CD = AB + (AC * math.sin(alpha))
r_dist = side_target_dist
l_dist = CD
angle_pid.update(r_dist, l_dist)
speed_pid.update(actual_dists[0], fwd_target_dist)
print('distances: [%s]' % ', '.join(map(str, actual_dists)))
speed = speed_pid.correction
if speed < 0:
speed *= 10
angle = angle_pid.correction
msg = drive_param()
msg.velocity = speed
msg.angle = angle
print('speed: %f' % speed)
print('angle: %f' % angle)
pub.publish(msg)
def update(self, xcontroller):
rospy.on_shutdown(self.offhook)
if xcontroller.power_button == 1:
rospy.signal_shutdown("X pressed")
if xcontroller.start_button == 1:
self.started = not self.started
print("ENABLED" if self.started else "DISABLED")
if self.started:
self.lmt_speed += xcontroller.dpad_y * 2
self.lmt_angle += -1 * xcontroller.dpad_x * 5
self.cur_angle = self.range_map(xcontroller.left_analog_x, 1, -1,
-1 * self.lmt_angle,
self.lmt_angle)
if xcontroller.left_trigger == 1 or xcontroller.right_trigger == 1:
if xcontroller.left_trigger != 1:
self.cur_speed = self.range_map(xcontroller.left_trigger,
-1, 1, -1 * self.lmt_speed,
0)
elif xcontroller.right_trigger != 1:
self.cur_speed = self.range_map(xcontroller.right_trigger,
1, -1, 0, self.lmt_speed)
else:
self.cur_speed = 0
if xcontroller.left_trigger != 1 and xcontroller.right_trigger != 1:
self.cur_speed = 0
if xcontroller.left_bumper == 1:
self.cur_angle = 0
if xcontroller.right_bumper == 1:
self.cur_speed = 0
else:
self.cur_speed = 0
self.cur_angle = 0
msg = drive_param()
msg.velocity = self.cur_speed
msg.angle = self.cur_angle
print('Limits: speed: %f, angle: %f' %
(self.lmt_speed, self.lmt_angle))
print('Current: speed: %f, angle: %f' %
(self.cur_speed, self.cur_angle))
self.pub.publish(msg)
def callback(message):
print("[#callback]: received message")
try:
image = bridge.imgmsg_to_cv2(message)
image = image[188:, 0:672, 0:3]
image = cv2.resize(image, (320, 160))
angle = float(model.predict(image[None, :, :, :], batch_size=1))
print('[#callback]: angle={}'.format(angle))
message = drive_param()
message.velocity = 16
message.angle = angle
control_drive_parameters.publish(message)
except CvBridgeError as e:
print(e)
def callback(data):
rospy.on_shutdown(offhook)
global angle
trigger_left = data.axes[2]
trigger_right = data.axes[5]
x = data.buttons[2]
y = data.buttons[3]
a = data.buttons[0]
b = data.buttons[1]
analog_left_x = data.axes[0]
if x == 1:
angle = -40
elif b == 1:
angle = 60
print('[#xjoystick_steer]: angle={}'.format(angle))
message = drive_param()
message.velocity = 0.0
message.angle = angle
control_drive_parameters.publish(message)
def callback(data):
rospy.on_shutdown(offhook)
global started
global max_speed
global max_angle
global lmt_speed
global lmt_angle
global cur_speed
global cur_angle
lefttrg = data.axes[2]
ritetrg = data.axes[5]
leftbump = data.buttons[4]
ritebump = data.buttons[5]
dpadx = data.axes[6]
dpady = data.axes[7]
leftanlgx = data.axes[0]
leftanlgy = data.axes[1]
start = data.buttons[0]
bigX = data.buttons[8]
if bigX == 1:
rospy.signal_shutdown("X pressed")
if start == 1:
started = not started
print("ENABLED" if started else "DISABLED")
if started:
lmt_speed += dpady * 2
lmt_angle += -1 * dpadx * 5
cur_angle = range_map(leftanlgx, 1, -1, -1 * lmt_angle, lmt_angle)
if lefttrg == 1 or ritetrg == 1:
if lefttrg != 1:
cur_speed = range_map(lefttrg, -1, 1, -1 * lmt_speed, 0)
elif ritetrg != 1:
cur_speed = range_map(ritetrg, 1, -1, 0, lmt_speed)
else:
cur_speed = 0
if lefttrg != 1 and ritetrg != 1:
cur_speed = 0
if leftbump == 1:
cur_angle = 0
if ritebump == 1:
cur_speed = 0
else:
cur_speed = 0
cur_angle = 0
msg = drive_param()
msg.velocity = cur_speed
msg.angle = cur_angle
print('Limits: speed: %f, angle: %f' % (lmt_speed, lmt_angle))
print('Current: speed: %f, angle: %f' % (cur_speed, cur_angle))
pub.publish(msg)
def offhook():
msg = drive_param()
msg.velocity = 0
msg.angle = 0
pub.publish(msg)
def offhook(self):
message = drive_param()
message.velocity = 0
message.angle = 0
self.pub.publish(message)
