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)