def table_position_control_thread(): bus = zmqmsgbus.Bus(sub_addr='ipc://ipc/source', pub_addr='ipc://ipc/sink') node = zmqmsgbus.Node(bus) time.sleep(1) # dont' start before robot position was acquired at least once robot_pos = node.recv('/position') while True: try: robot_pos = node.recv('/position', timeout=0) except queue.Empty: pass try: setpoint = node.recv('/left-arm/table-setpoint', timeout=0) setpoint = [int(setpoint[0])] + list(map_table_to_body_frame(*setpoint[1:], robot_pos=robot_pos)) node.publish('/left-arm/setpoint', setpoint) except queue.Empty: pass try: setpoint = node.recv('/right-arm/table-setpoint', timeout=0) setpoint = [int(setpoint[0])] + list(map_table_to_body_frame(*setpoint[1:], robot_pos=robot_pos)) node.publish('/right-arm/setpoint', setpoint) except queue.Empty: pass time.sleep(0.01)
def main(): global radius, theta global datagram_pos args = parse_args() config = json.loads(jsmin(args.config.read())) # zmqmsgbus subscribe bus = zmqmsgbus.Bus(sub_addr='ipc://ipc/source', pub_addr='ipc://ipc/sink') node = zmqmsgbus.Node(bus) theta = np.array(node.recv('/lidar/theta')) node.register_message_handler('/lidar/theta', update_scan_theta) datagram_pos = node.recv('/position') node.register_message_handler('/position', update_scan_pos) radius = np.array(node.recv('/lidar/radius')) node.register_message_handler('/lidar/radius', lambda topic, message: update_scan_radius(topic, message, args, config, node)) print('receiving') while True: time.sleep(1)
def setUp(self, ctx_mock): socket_mocks = {zmq.SUB: Mock(zmq.Socket), zmq.PUB: Mock(zmq.Socket)} ctx_mock.return_value.socket.side_effect = lambda arg: socket_mocks[arg] self.ctx_mock = ctx_mock self.bus = zmqmsgbus.Bus(sub_addr='ipc://ipc/source', pub_addr='ipc://ipc/sink') self.in_sock = socket_mocks[zmq.SUB] self.out_sock = socket_mocks[zmq.PUB]
def main(): bus = zmqmsgbus.Bus(sub_addr='ipc://ipc/source', pub_addr='ipc://ipc/sink') node = zmqmsgbus.Node(bus) position = PositionObject() handler = lambda topic, msg: position.set(msg) node.register_message_handler('/position', handler) handler = lambda topic, msg: proximity_beacon_msg(node, position, msg) node.register_message_handler('/proximity_beacon', handler) while True: time.sleep(1)
def main(): logging.basicConfig(level=logging.DEBUG) global got_lidar_fix bus = zmqmsgbus.Bus(sub_addr='ipc://ipc/source', pub_addr='ipc://ipc/sink') node = zmqmsgbus.Node(bus) pose = PositionEstimator() o = node.recv('/odometry_raw') pose.update_odometry(o) pose.reset([0.5, 0.5, np.pi / 2]) pose_lock = threading.Lock() def pose_reset_cb(reset_val): with pose_lock: pose.reset(reset_val) node.register_service('/position/reset', pose_reset_cb) t = threading.Thread(target=lidar_fix_led_thread, args=(node, )) t.start() while True: try: o = node.recv('/odometry_raw', timeout=0) with pose_lock: pose.update_odometry(o) node.publish('/position', pose.get_position()) except queue.Empty: pass try: l = node.recv('/lidar/position', timeout=0) with pose_lock: robot_position_lidar = l robot_position_kalman = pose.get_position() if (np.linalg.norm( np.array(robot_position_lidar[0:2]) - np.array(robot_position_kalman[0:2])) < 0.2 and abs( periodic_error(robot_position_lidar[2] - robot_position_kalman[2])) < 0.1): got_lidar_fix = True pose.update_lidar(robot_position_lidar) node.publish('/position', pose.get_position()) else: got_lidar_fix = False except queue.Empty: pass time.sleep(0.01)
def run(self): bus = zmqmsgbus.Bus(sub_addr=self.remote) for entry in self.variables: topic, var = entry.split(':') bus.subscribe(topic) while True: topic, data = bus.recv() print(topic, data) for idx, var in enumerate(self.variables): var_topic, var_path = var.split(':') if topic == var_topic: val = variable_path_extract_value(var_path, data) if val is not None: plot.lines[idx].updateData(val)
def main(): parser = argparse.ArgumentParser() args = parser.parse_args() bus = zmqmsgbus.Bus(sub_addr='ipc://ipc/source', pub_addr='ipc://ipc/sink') node = zmqmsgbus.Node(bus) global pose global pose_lock pose = RobotPose(xy=[0,0],theta=0) pose_lock = Lock() node.register_message_handler('/position', odometry_msg_handler) global target global target_lock target = None target_lock = Lock() node.register_message_handler('/waypoint', waypoint_msg_handler) obstacle_list = ObstacleList() handler = lambda topic, msg: obstacle_list.add(msg, time.time()) node.register_message_handler('/obstacle', handler) time.sleep(1) waypoint = WayPoint() while True: with target_lock: tg = copy.copy(target) if tg is not None: with pose_lock: pos = copy.copy(pose) obstacles = obstacle_list.get(time.time()) if obstacle_avoidance_robot_should_stop(pos, tg, obstacles): v_left, v_right = 0, 0 else: v_left, v_right = waypoint.process(pos, tg) node.call('/actuator/velocity', ['left-wheel', -v_left]) # left wheel velocity inversed node.call('/actuator/velocity', ['right-wheel', v_right]) obstacle_list.remove_old(time.time()) time.sleep(1/waypoint.frequency)
def main(): bus = zmqmsgbus.Bus(sub_addr='ipc://ipc/source', pub_addr='ipc://ipc/sink') node = zmqmsgbus.Node(bus) global pub global pub_lock pub = SimpleRPCActuatorPublisher(MASTER_BOARD_MSG_ADDR) print('publish messages to {}:{}'.format(*MASTER_BOARD_MSG_ADDR)) pub_lock = threading.Lock() node.register_service('/actuator/state', get_state) node.register_service('/actuator/voltage', update_voltage) node.register_service('/actuator/position', update_position) node.register_service('/actuator/velocity', update_velocity) node.register_service('/actuator/torque', update_torque) node.register_service('/actuator/trajectory', update_trajectory) while 1: time.sleep(0.02) with pub_lock: pub.publish(time.time())
def main(): parser = argparse.ArgumentParser("send parameters over zmqmsgbus") parser.add_argument("service", help="service to call") parser.add_argument("arg", help="YAML call arguments") parser.add_argument('--in', dest='from_addr', default='ipc://ipc/source') parser.add_argument('--out', dest='to_addr', default='ipc://ipc/sink') args = parser.parse_args() bus = zmqmsgbus.Bus(sub_addr=args.from_addr, pub_addr=args.to_addr) node = zmqmsgbus.Node(bus) while 1: try: a = yaml.load(args.arg) print('calling {} with {}'.format(args.service, a)) ret = node.call(args.service, a) print(ret) break except zmqmsgbus.call.CallFailed as e: print(e) time.sleep(0.1)
def main(): parser = argparse.ArgumentParser( "Sends the robot config to the master board.") parser.add_argument("config", help="YAML file containing robot config.") parser.add_argument("-w", "--watch", help="Watch config file for changes.", action="store_true") args = parser.parse_args() bus = zmqmsgbus.Bus(sub_addr='ipc://ipc/source', pub_addr='ipc://ipc/sink') node = zmqmsgbus.Node(bus) # wait until services list received time.sleep(1) send_config_file(node, open(args.config)) if args.watch: print("> watching for file changes...") old_mtime = os.path.getmtime(args.config) while True: try: try: mtime = os.path.getmtime(args.config) # Some editors delete the file before writing to it except FileNotFoundError: pass if mtime != old_mtime: old_mtime = mtime send_config_file(node, open(args.config)) time.sleep(0.1) except KeyboardInterrupt: break except: logging.exception("Unexpected error occured.")
def main(): logging.basicConfig(level=logging.DEBUG) bus = zmqmsgbus.Bus(sub_addr='ipc://ipc/source', pub_addr='ipc://ipc/sink') node = zmqmsgbus.Node(bus) time.sleep(4) logging.info("ready") zero_torque(node) team_color = None while team_color is None: team_color = init_sequence(node) wait_for_start(node) threading.Thread(target=kill_after_90_seconds, args=(node, )).start() logging.info("start!") # evade goldorak goto_waypoint(node, team_color, [0.66, 0.18, pi / 2]) time.sleep(2) # position behind blocks move_arm_in_body_frame(node, team_color, 'right', [0, 0.19, -0.06, 0.02, 0]) move_arm_in_body_frame(node, team_color, 'left', [0, 0.19, 0.06, 0.02, 0]) goto_waypoint(node, team_color, [0.9, 0.29, pi / 2]) time.sleep(0.5) # push blocks goto_waypoint(node, team_color, [0.9, 1.0, pi / 2]) time.sleep(0.5) goto_waypoint(node, team_color, [0.9, 0.97, pi / 2]) time.sleep(0.5) goto_waypoint(node, team_color, [0.9, 0.94, pi / 2]) move_arm_in_body_frame(node, team_color, 'right', [0, 0.19, -0.06, 0.15, 0]) move_arm_in_body_frame(node, team_color, 'left', [0, 0.19, 0.06, 0.15, 0]) # safe_arm_position(node) time.sleep(1) goto_waypoint(node, team_color, [0.9, 0.50, pi]) # close door 1 goto_waypoint(node, team_color, [0.4, 0.3, -pi]) set_waypoint(node, team_color, [0.23, 0.3, -pi]) time.sleep(2) goto_waypoint(node, team_color, [0.4, 0.3, 0]) # close door 2 goto_waypoint(node, team_color, [0.4, 0.6, -pi]) set_waypoint(node, team_color, [0.23, 0.6, -pi]) time.sleep(2) goto_waypoint(node, team_color, [0.4, 0.6, pi]) # get second block of elements goto_waypoint(node, team_color, [0.35, 0.9, pi / 2]) time.sleep(1) # sweep off tower move_arm_in_table_frame(node, team_color, 'left', [0, 0.068, 0.84, 0.14, 0]) set_pump(node, team_color, 'left', 1, 12) set_pump(node, team_color, 'left', 2, 12) set_pump(node, team_color, 'left', 3, 12) set_pump(node, team_color, 'left', 4, 12) time.sleep(2) # move above move_arm_in_table_frame(node, team_color, 'left', [0, 0.068, 0.88, 0.14, 0]) time.sleep(1) # grab blocks move_arm_in_table_frame(node, team_color, 'left', [0, 0.068, 0.88, 0.12, 0]) time.sleep(1) # move up move_arm_in_table_frame(node, team_color, 'left', [0, 0.068, 0.88, 0.21, 0]) time.sleep(1) # move arm behind move_arm_in_body_frame(node, team_color, 'left', [0, -.1, 0, 0.21, 0]) time.sleep(1) # turn around goto_waypoint(node, team_color, [0.35, 0.86, -pi / 2]) time.sleep(1) # do sweep movement to be sure move_arm_in_table_frame(node, team_color, 'right', [0, 0.068, 0.76, 0.13, 0]) time.sleep(2) move_arm_in_table_frame(node, team_color, 'right', [0, 0.068, 0.76, 0.08, 0]) set_pump(node, team_color, 'right', 1, 12) set_pump(node, team_color, 'right', 2, 12) set_pump(node, team_color, 'right', 3, 12) set_pump(node, team_color, 'right', 4, 12) time.sleep(1) # move above move_arm_in_table_frame(node, team_color, 'right', [0, 0.068, 0.88, 0.08, 0]) time.sleep(1) # grab blocks move_arm_in_table_frame(node, team_color, 'right', [0, 0.068, 0.88, 0.06, 0]) time.sleep(1) # move up move_arm_in_table_frame(node, team_color, 'right', [0, 0.068, 0.88, 0.21, 0]) time.sleep(1) # move arm in front move_arm_in_body_frame(node, team_color, 'right', [0, 0.18, -0.03, 0.21, 0]) time.sleep(1) # drop right goto_waypoint(node, team_color, [0.6, 1, pi / 2]) move_arm_in_table_frame(node, team_color, 'right', [0, 0.85, 1, 0.21, 0]) time.sleep(1) move_arm_in_table_frame(node, team_color, 'right', [0, 0.85, 1, 0.065, 0]) time.sleep(1) set_pump(node, team_color, 'right', 1, -12) set_pump(node, team_color, 'right', 2, -12) set_pump(node, team_color, 'right', 3, -12) set_pump(node, team_color, 'right', 4, -12) time.sleep(1) set_pump(node, team_color, 'right', 1, 0) set_pump(node, team_color, 'right', 2, 0) set_pump(node, team_color, 'right', 3, 0) set_pump(node, team_color, 'right', 4, 0) move_arm_in_body_frame(node, team_color, 'right', [0, 0.18, -0.03, 0.21, 0]) time.sleep(1) # drop left goto_waypoint(node, team_color, [0.6, 1, -pi / 2]) move_arm_in_table_frame(node, team_color, 'left', [0, 0.85, 1, 0.21, 0]) time.sleep(1) move_arm_in_table_frame(node, team_color, 'left', [0, 0.85, 1, 0.13, 0]) time.sleep(1) set_pump(node, team_color, 'left', 1, -12) set_pump(node, team_color, 'left', 2, -12) set_pump(node, team_color, 'left', 3, -12) set_pump(node, team_color, 'left', 4, -12) time.sleep(1) set_pump(node, team_color, 'left', 1, 0) set_pump(node, team_color, 'left', 2, 0) set_pump(node, team_color, 'left', 3, 0) set_pump(node, team_color, 'left', 4, 0) move_arm_in_body_frame(node, team_color, 'left', [0, -.1, 0, 0.21, 0]) time.sleep(1) # drop all elements time.sleep(10) set_pump(node, team_color, 'left', 1, 0) set_pump(node, team_color, 'left', 2, 0) set_pump(node, team_color, 'left', 3, 0) set_pump(node, team_color, 'left', 4, 0) set_pump(node, team_color, 'right', 1, 0) set_pump(node, team_color, 'right', 2, 0) set_pump(node, team_color, 'right', 3, 0) set_pump(node, team_color, 'right', 4, 0)
import zmqmsgbus bus = zmqmsgbus.Bus(sub_addr='ipc://ipc/source', pub_addr='ipc://ipc/sink') node = zmqmsgbus.Node(bus) print('receiving') while 1: print(node.recv('/example/counter'))
def test_constructor_calls_connect(self): sub_addr = 'ipc://ipc/source' pub_addr = 'ipc://ipc/sink' zmqmsgbus.Bus(sub_addr=sub_addr, pub_addr=pub_addr) self.in_sock.connect.assert_called_once_with(sub_addr) self.out_sock.connect.assert_called_once_with(pub_addr)
def __init__(self): self.bus = zmqmsgbus.Bus(sub_addr='ipc://ipc/source', pub_addr='ipc://ipc/sink') self.node = zmqmsgbus.Node(self.bus)
def main(): global position, heading global cloud_pts, cloud_pts_plot global red_cloud_pts global convexHulls global segments global ext_segments global corners global features args = parse_args() config = json.loads(jsmin(args.config.read())) # zmqmsgbus subscribe bus = zmqmsgbus.Bus(sub_addr='ipc://ipc/source', pub_addr='ipc://ipc/sink') node = zmqmsgbus.Node(bus) print('Waiting for registration to finish...') # position = node.recv('/lidar_testing/position') # node.register_message_handler('/lidar_testing/position', update_position) print('.') cloud_pts = node.recv('/lidar_viewer/cloud_pts') node.register_message_handler('/lidar_viewer/cloud_pts', update_cloud_pts) print('.') red_cloud_pts = node.recv('/lidar_viewer/red_cloud_pts') node.register_message_handler('/lidar_viewer/red_cloud_pts', update_red_cloud_pts) print('.') segments = node.recv('/lidar_viewer/segments') node.register_message_handler('/lidar_viewer/segments', update_segments) print('.') ext_segments = node.recv('/lidar_viewer/ext_segments') node.register_message_handler('/lidar_viewer/ext_segments', update_ext_segments) print('.') corners = node.recv('/lidar_viewer/corners') node.register_message_handler('/lidar_viewer/corners', update_corners) print('.') features = node.recv('/lidar_viewer/features') node.register_message_handler('/lidar_viewer/features', update_features) print('-- ok') # PyQtGraph stuff app = QtGui.QApplication([]) pg.setConfigOptions(antialias=False) plot = pg.plot(title='Lidar Polar Plot') plot.resize(600,600) plot.setAspectLocked() print('Plotting...') while 1: time.sleep(1) plot.clear() PlotPolar(plot) cloud_size = red_cloud_pts.shape[0] plot.plot(red_cloud_pts[np.arange(0,cloud_size,4),0],red_cloud_pts[np.arange(0,cloud_size,4),1],pen=None,symbol='o', symbolPen=None,symbolSize=7,symbolBrush=(255,255,255,50)) linePen = pg.mkPen(color=(200,200,200,200),width=2, style=QtCore.Qt.DotLine) colors=[(255,0,0,180),(0,255,0,180),(0,0,255,180),(255,255,0,180), (255,0,255,180),(0,127,255,180),(127,0,0,180),(0,127,0,180), (0,0,127,180),(127,255,0,180),(127,0,255,180),(0,127,255,180)] # # draw segments # try: # for idx in range(segments.shape[0]): # linePen = pg.mkPen(color=colors[idx],width=4, # style=QtCore.Qt.SolidLine) # a = plot.plot((segments[idx][0][0], segments[idx][0][1]), # (segments[idx][1][0], segments[idx][1][1]), pen=linePen) # except: # pass # draw external segments try: for idx in range(ext_segments.shape[0]): linePen = pg.mkPen(color=colors[idx],width=4, style=QtCore.Qt.SolidLine) a = plot.plot((ext_segments[idx][0][0], ext_segments[idx][0][1]), (ext_segments[idx][1][0], ext_segments[idx][1][1]), pen=linePen) except: pass # draw corners found symbolePen = pg.mkPen(color=(255,255,0,255), width=4) if corners.shape[0] > 0: plot.plot(corners[:,0], corners[:,1], pen=None, symbol='x', symbolPen=symbolePen) # draw found feature # symbolePen = pg.mkPen(color=(255,255,0,255), width=4) # print(features.shape) # if features.shape[0] > 0: # plot.plot(features[:,0], features[:,1], pen=None, symbol='x', # symbolPen=symbolePen) # # draw table estimation corner # table_corner = np.array([[0,0],[0,config['TABLE_HEIGHT']],[config['TABLE_WIDTH'], # config['TABLE_HEIGHT']],[config['TABLE_WIDTH'],0]], dtype=float) # table_corner = table_corner - [position[0], position[1]] # table_corner = rotatePolygon(table_corner, -heading+math.pi/2) # symbolePen = pg.mkPen(color=(255,255,255,255), width= 2) # plot.plot(table_corner[:,0], table_corner[:,1], # pen=None, symbol='x', symbolPen=symbolePen, symbolSize=12) app.processEvents() QtGui.QApplication.instance().exec_()
def main(): logging.basicConfig(level=logging.DEBUG) bus = zmqmsgbus.Bus(sub_addr='ipc://ipc/source', pub_addr='ipc://ipc/sink') node = zmqmsgbus.Node(bus) time.sleep(1) offsets = yaml.load(open(sys.argv[1])) left_arm_config = yaml.load(open("config/config-left-arm.yaml")) right_arm_config = yaml.load(open("config/config-right-arm.yaml")) global l, r, arm_lock arm_lock = threading.Lock() l = Arm(negative_elbow_angle=True) r = Arm() def zero_homing(arg): global l, r, arm_lock logging.debug('perform zero homing') with arm_lock: endstopper = homing_handler.Endstopper() endstopper.add(['left-z'], 30, left_arm_config['actuator']['left-z']['control']['torque_limit']) endstopper.add(['right-z'], 30, right_arm_config['actuator']['right-z']['control']['torque_limit']) endstopper_zeros = endstopper.start() l.set_zeros({a[len('left-'):]: z * offsets[a + '-dir'] for a, z in endstopper_zeros.items() if 'left-' in a}) r.set_zeros({a[len('right-'):]: z * offsets[a + '-dir'] for a, z in endstopper_zeros.items() if 'right-' in a}) def zero_sequence(side): global l, r if side == 'left': arm = l else: arm = r arm.set_zeros({'shoulder': cst_vel_homing.homing(side+'-shoulder', 2) * offsets[side+'-shoulder-dir']}) arm.set_zeros({'elbow': cst_vel_homing.homing(side+'-elbow', 2) * offsets[side+'-elbow-dir']}) arm.set_zeros({'wrist': cst_vel_homing.homing(side+'-wrist', 2, periodic=True) * offsets[side+'-wrist-dir']}) left = threading.Thread(target=zero_sequence, args=('left',)) left.start() right = threading.Thread(target=zero_sequence, args=('right',)) right.start() left.join() right.join() l.move_hand(0.212, 0, 0.18, 0) r.move_hand(0.212, 0, 0.18, 0) logging.debug('left hand zeros: {}'.format(l.zeros)) logging.debug('right hand zeros: {}'.format(r.zeros)) actuator_position(node, 'left', l.get_actuators(), offsets) actuator_position(node, 'right', r.get_actuators(), offsets) node.register_service('/arm/run_zero_homing', zero_homing) table_pos_control = threading.Thread(target=table_position_control_thread) table_pos_control.start() while True: try: setpoint = node.recv('/left-arm/setpoint', timeout=0) with arm_lock: setpoint = [int(setpoint[0])] + list(map_body_to_arm_frame(*[float(v) for v in setpoint[1:]], arm='left')) if setpoint[0] == 0: l.move_hand(*setpoint[1:]) else: l.move_tcp(*setpoint) actuator_position(node, 'left', l.get_actuators(), offsets) except queue.Empty: pass except ValueError as e: logging.debug('left arm ' + str(e)) try: setpoint = node.recv('/right-arm/setpoint', timeout=0) with arm_lock: setpoint = [int(setpoint[0])] + list(map_body_to_arm_frame(*[float(v) for v in setpoint[1:]], arm='right')) if setpoint[0] == 0: r.move_hand(*setpoint[1:]) else: r.move_tcp(*setpoint) actuator_position(node, 'right', r.get_actuators(), offsets) except queue.Empty: pass except ValueError as e: logging.debug('right arm ' + str(e)) time.sleep(0.01)
def homing(actuator, velocity, periodic=False): bus = zmqmsgbus.Bus(sub_addr='ipc://ipc/source', pub_addr='ipc://ipc/sink') node = zmqmsgbus.Node(bus) time.sleep(1) node.call('/actuator/velocity', [actuator, 0]) node.call( '/actuator/config_update', {'actuator': { actuator: { 'stream': { 'index': 10, 'motor_pos': 30 } } }}) state = { 'vel': 0, 'idx_p': None, 'idx_n': None, 'idx_seq_nbr': float('inf') } def vel_cb(topic, msg): state['vel'] = msg def idx_cb(topic, msg): seq = msg[1] if seq > state['idx_seq_nbr']: # new index if abs(state['vel']) > velocity * 0.1: if state['vel'] > 0: state['idx_p'] = msg[0] else: state['idx_n'] = msg[0] state['idx_seq_nbr'] = seq node.register_message_handler('/actuator/' + actuator + '/velocity', vel_cb) node.register_message_handler('/actuator/' + actuator + '/index', idx_cb) n = 0.3 while True: node.call('/actuator/velocity', [actuator, velocity]) time.sleep(n) if state['idx_p'] is not None and state['idx_n'] is not None: break node.call('/actuator/velocity', [actuator, -velocity]) time.sleep(2 * n) if state['idx_p'] is not None and state['idx_n'] is not None: break node.call('/actuator/velocity', [actuator, velocity]) time.sleep(n) if state['idx_p'] is not None and state['idx_n'] is not None: break n *= 2 if periodic: center = center_between_periodic_angles(state['idx_p'], state['idx_n']) else: center = (state['idx_p'] + state['idx_n']) / 2 node.call('/actuator/position', [actuator, center]) node.call( '/actuator/config_update', {'actuator': { actuator: { 'stream': { 'index': 0, 'motor_pos': 0 } } }}) return center