def main(): # Parse CLI args args = setup_cli_args(lidar_cli.max_linear, lidar_cli.max_angular, lidar_cli.stop_angle, lidar_cli.publish_rate, lidar_cli.centroid_topic, lidar_cli.vel_topic, cli.log_level) # Setup logging setup_logging(level=args[LOG_LEVEL]) rospy.init_node('teleop_node') teleop = LidarTeleop(max_linear=args[MAX_LINEAR], max_angular=args[MAX_ANGULAR], full_stop_angle=args[STOP_ANGLE], publish_rate=args[PUBLISH_RATE], centroid_topic=args[CENTROID_TOPIC], vel_topic=args[VEL_TOPIC]) rospy.loginfo("Running") try: # Running this in a thread will enable Ctrl+C exits Thread(target=teleop.perform_teleop).start() rospy.spin() except KeyboardInterrupt: pass finally: teleop.stop() teleop.send_stop() rospy.loginfo("Exiting")
def main(): # Parse CLI args args = setup_cli_args(ImageServer.args, cli.camera_name_optional, fifo_name_option, fifo_verbose, cli.log_level) # Setup logging setup_logging(level=args[LOG_LEVEL]) image_server = ImageServer(template_file=args[TEMPLATE_FILE], image_x=args[IMAGE_X], image_y=args[IMAGE_Y], camera_name=args[CAMERA_NAME], http_port=args[HTTP_PORT], http_delay_secs=args[HTTP_DELAY_SECS], http_verbose=args[HTTP_VERBOSE]) fifo_reader = FifoReader(image_server=image_server, fifo_name=args[FIFO_NAME], fifo_verbose=args[FIFO_VERBOSE]) try: image_server.start() fifo_reader.start() sleep() except KeyboardInterrupt: pass finally: image_server.stop() fifo_reader.stop() logger.info("Exiting") sys.exit(1)
def main(): # Parse CLI args args = setup_cli_args(cli.grpc_host, cli.log_level) # Setup logging setup_logging(level=args[LOG_LEVEL]) http = Flask(__name__) @http.route("/count") def val_count(): global count return "Read {0} values".format(count) def read_values(): global count while True: print("Got location: {0}".format(client.get_xy())) count += 1 # Start client with LocationClient(args[GRPC_HOST]) as client: # Run read_values in a thread count = 0 Thread(target=read_values).start() # Run HTTP server in a thread Thread(target=http.run, kwargs={"port": 8080}).start() sleep()
def main(): # Parse CLI args args = setup_cli_args(lidar_cli.slice_size, lidar_cli.slice_offset, lidar_cli.max_dist_mult, lidar_cli.publish_rate, lidar_cli.scan_topic, lidar_cli.contour_topic, lidar_cli.centroid_topic, lidar_cli.publish_pc, lidar_cli.pc_topic, cli.log_level) # Setup logging setup_logging(level=args[LOG_LEVEL]) rospy.init_node('geometry_node') geometry = LidarGeometry(slice_size=args[SLICE_SIZE], slice_offset=args[SLICE_OFFSET], max_dist_mult=args[MAX_DIST_MULT], publish_rate=args[PUBLISH_RATE], publish_pc=args[PUBLISH_PC], scan_topic=args[SCAN_TOPIC], contour_topic=args[CONTOUR_TOPIC], centroid_topic=args[CENTROID_TOPIC], pc_topic=args[PC_TOPIC]) rospy.loginfo("Running") try: # Running this in a thread will enable Ctrl+C exits Thread(target=geometry.eval_points).start() rospy.spin() except KeyboardInterrupt: pass finally: geometry.stop() rospy.loginfo("Exiting")
def main(): # Parse CLI args args = setup_cli_args(cli.grpc_host, cli.mqtt_host, cli.camera_name, cli.log_level) # Setup logging setup_logging(level=args[LOG_LEVEL]) # Start location reader with LocationClient(args[GRPC_HOST]) as loc_client: # Define MQTT callbacks def on_connect(mqtt_client, userdata, flags, rc): logger.info("Connected with result code: {0}".format(rc)) Thread(target=publish_locations, args=(mqtt_client, userdata)).start() def publish_locations(mqtt_client, userdata): while True: x_loc, y_loc = loc_client.get_xy() if x_loc is not None and y_loc is not None: result, mid = mqtt_client.publish("{0}/x".format( userdata[CAMERA_NAME]), payload=x_loc[0]) result, mid = mqtt_client.publish("{0}/y".format( userdata[CAMERA_NAME]), payload=y_loc[0]) # Setup MQTT client with MqttConnection(args[MQTT_HOST], userdata={CAMERA_NAME: args[CAMERA_NAME]}, on_connect=on_connect): waitForKeyboardInterrupt() logger.info("Exiting...")
def main(): # Parse CLI args args = setup_cli_args( cli.width, cli.usb_camera, # cli.usb_port, cli.display, cli.flip_x, cli.flip_y, cli.http_host, cli.http_file, cli.http_delay_secs, cli.http_verbose, cli.log_level) # Setup logging setup_logging(level=args[LOG_LEVEL]) color_picker = ColorPicker(**strip_loglevel(args)) try: color_picker.start() except KeyboardInterrupt: pass finally: color_picker.stop() logger.info("Exiting...")
def main(): # Parse CLI args args = setup_cli_args(cli.grpc_host, cli.mqtt_host, cli.led_name, cli.led_brightness, cli.log_level) # Setup logging setup_logging(level=args[LOG_LEVEL]) blinkt = BlinktSubscriber(args[LED_BRIGHTNESS]) # Define MQTT callbacks def on_connect(mqtt_client, userdata, flags, rc): logger.info("Connected with result code: {0}".format(rc)) mqtt_client.subscribe("leds/{0}".format(userdata[TOPIC])) def on_message(mqtt_client, userdata, msg): logger.info("{0} {1}".format(msg.topic, msg.payload)) # Setup MQTT client with MqttConnection(args[MQTT_HOST], userdata={TOPIC: args[LED_NAME]}, on_connect=on_connect, on_message=on_message): waitForKeyboardInterrupt() logger.info("Exiting...")
def main(): parser = argparse.ArgumentParser() parser.add_argument("-s", "--serial", default="ttyACM0", type=str, help="Arduino serial port [ttyACM0] (OSX is cu.usbmodemXXXX)") cli.grpc_host(parser) parser.add_argument("-x", "--xservo", default=5, type=int, help="X servo PWM pin [5]") parser.add_argument("-y", "--yservo", default=6, type=int, help="Y servo PWM pin [6]") cli.alternate(parser) cli.calib(parser) cli.log_level(parser) args = vars(parser.parse_args()) alternate = args["alternate"] calib = args["calib"] xservo = args["xservo"] yservo = args["yservo"] setup_logging(level=args[LOG_LEVEL]) # Setup firmata client port = ("" if is_windows() else "/dev/") + args["serial"] try: board = Arduino(port) logger.info("Connected to Arduino at: {0}".format(port)) except OSError as e: logger.error("Failed to connect to Arduino at {0} - [{1}]".format(port, e)) sys.exit(0) with LocationClient(args[GRPC_HOST]) as client: # Create servos servo_x = FirmataServo("Pan", alternate, board, "d:{0}:s".format(xservo), 1.0, 8) servo_y = FirmataServo("Tilt", alternate, board, "d:{0}:s".format(yservo), 1.0, 8) try: if calib: try: calib_t = Thread(target=calibrate_servo.calibrate, args=(client, servo_x, servo_y)) calib_t.start() calib_t.join() except KeyboardInterrupt: pass else: if alternate: # Set servo X to go first servo_x.ready_event.set() try: servo_x.start(True, lambda: client.get_x(), servo_y.ready_event if not calib else None) servo_y.start(False, lambda: client.get_y(), servo_x.ready_event if not calib else None) servo_x.join() servo_y.join() except KeyboardInterrupt: pass finally: servo_x.stop() servo_y.stop() finally: board.exit() logger.info("Exiting...")
def on_connect(mqtt_client, userdata, flags, rc): # Subscribe to all broker messages topic = userdata[TOPIC] mqtt_client.subscribe(topic) setup_logging(filename=args[LOG_FILE], format="%(asctime)s %(levelname)-6s %(message)s", level=logging.DEBUG) logger.info("Connected, subscribing to topic %s", topic)
def main(): # Parse CLI args args = setup_cli_args(lidar_cli.iterations, lidar_cli.min_num_points, lidar_cli.threshold_distance, lidar_cli.pause, lidar_cli.plot_all, lidar_cli.plot_points, lidar_cli.plot_centroid, lidar_cli.max_axis_mult, lidar_cli.contour_topic, ImageServer.args, cli.log_level) # Setup logging setup_logging(level=args[LOG_LEVEL]) rospy.init_node('walls_node') image_server = ImageServer(template_file=args[TEMPLATE_FILE], http_port=args[HTTP_PORT], http_delay_secs=args[HTTP_DELAY_SECS], http_verbose=args[HTTP_VERBOSE], log_info=rospy.loginfo, log_debug=rospy.logdebug, log_error=rospy.logerror) image_server.start() image = WallDetector(image_server=image_server, iterations=args[ITERATIONS], min_points=args[MIN_POINTS], threshold=args[THRESHOLD], pause=args[PAUSE], plot_all=args[PLOT_ALL], plot_points=args[PLOT_POINTS], plot_centroid=args[PLOT_CENTROID], max_axis_mult=args[MAX_AXIS_MULT]) rospy.loginfo("Running") try: # Running this in a thread will enable Ctrl+C exits Thread(target=image.generate_image).start() rospy.spin() except KeyboardInterrupt: pass finally: image.stop() image_server.stop() rospy.loginfo("Exiting")
def main(): # Parse CLI args args = setup_cli_args(cli.grpc_host, cli.log_level) # Setup logging setup_logging(level=args[LOG_LEVEL]) # Start location client with LocationClient(args[GRPC_HOST]) as client: stream_ids = tls.get_credentials_file()['stream_ids'] stream_id = stream_ids[0] # Declare graph graph = go.Scatter(x=[], y=[], mode='lines+markers', stream=dict(token=stream_id, maxpoints=80)) data = go.Data([graph]) layout = go.Layout(title='Target Locations', xaxis=go.XAxis(range=[0, 800]), yaxis=go.YAxis(range=[0, 450])) fig = go.Figure(data=data, layout=layout) py.plot(fig, filename='plot-locations') # Write data stream = py.Stream(stream_id) stream.open() logger.info("Opening plot.ly tab") time.sleep(5) try: while True: x_val, y_val = client.get_xy() if x_val[0] == -1 or y_val[0] == -1: continue x = x_val[1] - abs(x_val[1] - x_val[0]) y = abs(y_val[1] - y_val[0]) stream.write(dict(x=x, y=y)) time.sleep(.10) except KeyboardInterrupt: pass finally: stream.close() logger.info("Exiting...")
def main(): # Parse CLI args args = setup_cli_args(cli.grpc_host, cli.log_level) # Setup logging setup_logging(level=args[LOG_LEVEL]) with LocationClient(args[GRPC_HOST]) as client: try: while True: print("Got location: {0}".format(client.get_xy())) except KeyboardInterrupt: pass logger.info("Exiting...")
def main(): # Parse CLI args args = setup_cli_args(lidar_cli.plot_all, lidar_cli.plot_points, lidar_cli.plot_contour, lidar_cli.plot_centroid, lidar_cli.plot_slices, lidar_cli.max_axis_mult, lidar_cli.contour_topic, ImageServer.args, cli.log_level) # Setup logging setup_logging(level=args[LOG_LEVEL]) rospy.init_node('contour_node') image_server = ImageServer(template_file=args[TEMPLATE_FILE], http_port=args[HTTP_PORT], http_delay_secs=args[HTTP_DELAY_SECS], http_verbose=args[HTTP_VERBOSE], log_info=rospy.loginfo, log_debug=rospy.logdebug, log_error=rospy.logerror) image_server.start() image = LidarImage(image_server=image_server, plot_all=args[PLOT_ALL], plot_points=args[PLOT_POINTS], plot_contour=args[PLOT_CONTOUR], plot_centroid=args[PLOT_CENTROID], plot_slices=args[PLOT_SLICES], max_axis_mult=args[MAX_AXIS_MULT]) rospy.loginfo("Running") try: # Running this in a thread will enable Ctrl+C exits Thread(target=image.generate_image).start() rospy.spin() except KeyboardInterrupt: pass finally: image.stop() image_server.stop() rospy.loginfo("Exiting")
def main(): # Parse CLI args args = setup_cli_args(cli.log_level) # Setup logging setup_logging(level=args[LOG_LEVEL]) rospy.init_node('stop_node') rospy.loginfo("Running") vel_topic = "/cmd_vel" rospy.loginfo("Publishing Twist vals to topic {}".format(vel_topic)) vel_pub = rospy.Publisher(vel_topic, Twist, queue_size=5) rospy.sleep(1) rospy.loginfo("Stopping the robot") vel_pub.publish(new_twist(0, 0)) rospy.sleep(1) rospy.loginfo("Exiting")
def main(): # Parse CLI args args = setup_cli_args(CameraImageSource.args, ColorPicker.args, cli.log_level) # Setup logging setup_logging(level=args[LOG_LEVEL]) image_source = CameraImageSource(usb_camera=args[USB_CAMERA], usb_port=args[USB_PORT]) color_picker = ColorPicker(image_source=image_source, width=args[WIDTH], flip_x=args[FLIP_X], flip_y=args[FLIP_Y]) try: image_source.start() color_picker.run() except KeyboardInterrupt: pass finally: image_source.stop() logger.info("Exiting")
def main(): # Parse CLI args args = setup_cli_args(cli.grpc_host, cli.log_level) setup_logging(level=args[LOG_LEVEL]) with LocationClient(args[GRPC_HOST]) as client: init_w, init_h = 800, 450 root = tk.Tk() canvas = tk.Canvas(root, bg="white", width=init_w, height=init_h) canvas.pack() sketch = LocationSketch(canvas) b = tk.Button(root, text="Clear", command=sketch.clear_canvas) b.pack(side=tk.LEFT) lb_var = tk.IntVar() lb_var.set(1) lb = tk.Checkbutton(root, text="Lines", variable=lb_var, command=sketch.toggle_lines) lb.pack(side=tk.LEFT) pb_var = tk.IntVar() pb_var.set(1) pb = tk.Checkbutton(root, text="Points", variable=pb_var, command=sketch.toggle_points) pb.pack(side=tk.LEFT) Thread(target=sketch.plot_vals, args=(client, init_w, init_h)).start() root.mainloop()
def main(): setup_logging() with LocationClient("localhost") as client: for i in range(1000): logger.info("Read value: {0}".format(client.get_xy())) logger.info("Exiting...")
from arc852.image_server import ImageServer from arc852.object_tracker import ObjectTracker from arc852.utils import setup_logging from multi_object_filter import MultiObjectFilter logger = logging.getLogger(__name__) if __name__ == "__main__": # Parse CLI args args = setup_cli_args(CameraImageSource.args, ImageServer.args, cli.camera_name_optional, MultiObjectFilter.args, ObjectTracker.args, cli.log_level) # Setup logging setup_logging(level=args[LOG_LEVEL]) image_source = CameraImageSource(usb_camera=args[USB_CAMERA], usb_port=args[USB_PORT]) image_server = ImageServer(template_file=args[TEMPLATE_FILE], camera_name=args[CAMERA_NAME], http_host=args[HTTP_HOST], http_delay_secs=args[HTTP_DELAY_SECS], http_verbose=args[HTTP_VERBOSE]) tracker = ObjectTracker(image_source=image_source, image_server=image_server, display=args[DISPLAY], width=args[WIDTH], middle_percent=args[MIDDLE_PERCENT],
mqtt_client.subscribe(topic) def on_message(mqtt_client, userdata, msg): # Payload is a string byte array val = bytes.decode(msg.payload) logger.info("%s : %s", msg.topic, val) if __name__ == "__main__": # Parse CLI args parser = argparse.ArgumentParser() cli.mqtt_host(parser) parser.add_argument("-d", "--dir", dest=DIR, required=True, help="Log directory") cli.log_level(parser) args = vars(parser.parse_args()) # Setup logging filename = "{0}/robot-{1}.log".format(args[DIR], datetime.datetime.now().strftime("%a-%b-%d-%Y-%H-%M-%S")) print("Writing log data to {0}".format(filename)) setup_logging(filename=filename, level=args[LOG_LEVEL]) # Setup MQTT client with MqttConnection(args[MQTT_HOST], userdata={}, on_connect=on_connect, on_message=on_message): waitForKeyboardInterrupt() logger.info("Exiting...")
import logging import time from arc852.utils import setup_logging from rosbridge_client import RosBridgeClient logger = logging.getLogger(__name__) setup_logging() def main(): bridge = RosBridgeClient("localhost:50051") count = 100 # Non-streaming version start1 = time.time() for i in range(count): twist_data = RosBridgeClient.newTwistData(i, i + 3) bridge.write_twist(twist_data) end1 = time.time() logger.info("Non-streaming elapsed: " + str(end1 - start1)) # Streaming version def twist_gen(cnt): for i in range(cnt): twist_data = RosBridgeClient.newTwistData(i, i + 2) yield twist_data start2 = time.time() bridge.stream_twist(twist_gen(count))
def main(): # Parse CLI args p = argparse.ArgumentParser() cli.usb_camera(p), cli.width(p), cli.middle_percent(p), cli.minimum_pixels(p), cli.hsv_range(p), cli.leds(p), cli.flip_x(p), cli.flip_y(p), cli.mask_x(p), cli.mask_y(p), cli.vertical_lines(p), cli.horizontal_lines(p), cli.camera_name_optional(p), cli.display(p), cli.draw_contour(p), cli.draw_box(p), cli.http_host(p), cli.http_delay_secs(p), cli.http_file(p), cli.http_verbose(p), p.add_argument("--dualbgr", dest=DUAL_BGR, required=True, help="Dual color BGR value") p.add_argument("--singlebgr", dest=SINGLE_BGR, required=True, help="Single color BGR value") p.add_argument("--dualport", dest=DUAL_PORT, default=GRPC_PORT_DEFAULT, type=int, help="Dual gRPC port [{0}]".format(GRPC_PORT_DEFAULT)) p.add_argument("--singleport", dest=SINGLE_PORT, default=GRPC_PORT_DEFAULT + 1, type=int, help="Dual gRPC port [{0}]".format(GRPC_PORT_DEFAULT + 1)) cli.log_level(p) args = vars(p.parse_args()) # Setup logging setup_logging(level=args[LOG_LEVEL]) tracker = ObjectTracker(width=args[WIDTH], middle_percent=args[MIDDLE_PERCENT], display=args[DISPLAY], flip_x=args[FLIP_X], flip_y=args[FLIP_Y], mask_x=args[MASK_X], mask_y=args[MASK_Y], usb_camera=args[USB_CAMERA], usb_port=args[USB_PORT], camera_name=args[CAMERA_NAME], http_host=args[HTTP_HOST], http_file=args[HTTP_FILE], http_delay_secs=args[HTTP_DELAY_SECS], http_verbose=args[HTTP_VERBOSE]) dual_filter = DualObjectFilter(tracker, bgr_color=args[DUAL_BGR], hsv_range=args[HSV_RANGE], minimum_pixels=args[MINIMUM_PIXELS], grpc_port=args[DUAL_PORT], leds=False, display_text=False, draw_contour=args[DRAW_CONTOUR], draw_box=args[DRAW_BOX], vertical_lines=args[VERTICAL_LINES], horizontal_lines=args[HORIZONTAL_LINES]) single_filter = SingleObjectFilter(tracker, bgr_color=args[SINGLE_BGR], hsv_range=args[HSV_RANGE], minimum_pixels=args[MINIMUM_PIXELS], grpc_port=args[SINGLE_PORT], leds=False, display_text=True, draw_contour=args[DRAW_CONTOUR], draw_box=args[DRAW_BOX], vertical_lines=False, horizontal_lines=False) try: tracker.start(single_filter, dual_filter) except KeyboardInterrupt: pass finally: tracker.stop() logger.info("Exiting...")