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...")
