def connect_to_channels(self): # try to connect to communication channels #os.sched_setscheduler(0, os.SCHED_FIFO, os.sched_param(35)) self.broker_server = pab.broker(self.broker_server_id) self.broker_server.request_signal(self.state_channel_id, self.state_msg_type) self.broker_server.request_signal(self.image_channel_id, self.image_msg_type) self.broker_server.request_signal(self.lidar_channel_id, self.lidar_msg_type)
def initialize(addr_broker_ip="tcp://localhost:51468", realsense = False, lidar = False): broker = pab.broker(addr_broker_ip) #broker.register_signal("franka_target_pos", pab.MsgType.target_pos) #broker.register_signal("franka_des_tau", pab.MsgType.des_tau) broker.request_signal("franka_state", pab.MsgType.franka_state, True) if realsense: broker.request_signal("realsense_images", pab.MsgType.realsense_image) if lidar: broker.request_signal("franka_lidar", pab.MsgType.franka_lidar) time.sleep(1) return broker
def main(): signal.signal(signal.SIGINT, signal_handler) broker = pab.broker("tcp://10.250.144.21:51468") broker.register_signal("franka_target_pos", pab.MsgType.target_pos) # input('st 6 in xpanda...') broker.request_signal("franka_state", pab.MsgType.franka_state) while (True): msg = broker.recv_msg("franka_state", -1) data = get_state(msg) store.append(data)
def main(): signal.signal(signal.SIGINT, signal_handler) broker = pab.broker("tcp://10.250.144.21:51468") broker.request_signal("franka_lidar", pab.MsgType.franka_lidar) while (True): data = dict() msg = broker.recv_msg("franka_lidar", -1) data['systemtime'] = datetime.datetime.now() data['data'] = msg.get_data() # print(data['data']) data['timestamp'] = msg.get_timestamp() data['fnumber'] = msg.get_fnumber() store.append(data)
def connect_to_channels(self, IMG_ON=False): # try to connect to communication channels # os.sched_setscheduler(0, os.SCHED_FIFO, os.sched_param(35)) self.broker_server = pab.broker(self.broker_server_id) self.broker_server.request_signal(self.state_channel_id, self.state_msg_type) if IMG_ON: self.broker_server.request_signal(self.image_channel_id, self.image_msg_type) if True: print("Connection setup worked!")
def default(): broker = pab.broker() print(broker.register_signal("franka_target_pos", pab.MsgType.target_pos)) time.sleep(0.5) # print(broker.request_signal("franka_lidar", pab.MsgType.franka_lidar)) time.sleep(0.5) print(broker.request_signal("franka_state", pab.MsgType.franka_state)) time.sleep(0.5) print("Register signal <_des_tau> {}".format( broker.register_signal("franka_des_tau", pab.MsgType.des_tau))) time.sleep(0.5) # print(broker.request_signal("realsense_images", pab.MsgType.realsense_image)) time.sleep(0.5) return broker
def grab_image(): broker = pab.broker("tcp://localhost:51468") broker.request_signal("realsense_images", pab.MsgType.realsense_image) img1 = broker.recv_msg("realsense_images", -1) img1_rgb = img1.get_rgb() img1_rgbshape = img1.get_shape_rgb() img1_rgb = img1_rgb.reshape(img1_rgbshape) img1_depth = img1.get_depth() img1_depthshape = img1.get_shape_depth() img1_depth = img1_depth.reshape(img1_depthshape) return img1_rgb, img1_depth
def main(): signal.signal(signal.SIGINT, signal_handler) broker = pab.broker("tcp://10.250.144.21:51468") broker.request_signal("realsense_images", pab.MsgType.realsense_image) while (True): msg = broker.recv_msg("realsense_images", -1) data = dict() data['systemtime'] = datetime.datetime.now() data['timestamp'] = msg.get_timestamp() data['fnumber'] = msg.get_fnumber() rgbdata = msg.get_rgb() data['rgbdata'] = rgbdata.reshape(msg.get_shape_rgb()) depthdata = msg.get_depth() data['depthdata'] = depthdata.reshape(msg.get_shape_depth()) store.append(data)
def grab_image(broker=None): if not broker: # either do these initialization steps inside or outside function, but either way happens just once broker = pab.broker("tcp://localhost:51468") broker.request_signal("realsense_images", pab.MsgType.realsense_image) img1 = broker.recv_msg("realsense_images", -1) img1_rgb = img1.get_rgb() img1_rgbshape = img1.get_shape_rgb() img1_rgb = img1_rgb.reshape(img1_rgbshape) img1_depth = img1.get_depth() img1_depthshape = img1.get_shape_depth() img1_depth = img1_depth.reshape(img1_depthshape) return img1_rgb, img1_depth
import time import numpy as np import py_at_broker as pab from PIL import Image b = pab.broker() print(b.request_signal("franka_lidar", pab.MsgType.franka_lidar)) print(b.request_signal("realsense_images", pab.MsgType.realsense_image)) counter = 0 start = time.time() lidar_list = [] img_counter = 0 time2go = 1.0 lidar_list = [] c = 0 n_samples = 50 img_counter = 0 for n in range(10): print("press Enter") i = input() try: c = 0 while c < n_samples: lidar = b.recv_msg("franka_lidar", -1)
target_th_null = np.zeros(7, dtype=np.float64) target_th_null[3] = -1.55 target_th_null[5] = 1.9 # Max deviation between current and desired Cart pos (to avoid integration to inf) maxCartDev = 0.2 # in m max_sync_jitter = 0.2 use_inv_dyn = False robot_name = "franka" # for use in signals try: os.sched_setscheduler(0, os.SCHED_FIFO, os.sched_param(35)) except OSError as err: print("Error: Failed to set proc. to real-time scheduler\n{0}".format(err)) ## Register Signals b = pab.broker() b.register_signal(robot_name + "_des_tau", pab.MsgType.des_tau) # b.request_signal("task_policy", pab.MsgType.task_policy, True) b.request_signal(robot_name + "_state", pab.MsgType.franka_state, True) b_lidar = pab.broker() print("Before Lidar") b_lidar.request_signal("franka_lidar", pab.MsgType.franka_lidar, -1) print("Setup completed. Starting...") # set maximum distances p0 = np.array([0.3, 0.3, 0.3]) sigma_ddq_null = 0.5 * np.identity(7) sigma_ddx_ee = 0.3 * np.identity(3) # space mouse first counter = 0 # Frame number initCounter = 0 # For interpolating to target
import py_at_broker as pab import matplotlib.pyplot as plt import time import numpy as np d = pab.broker("tcp://10.250.144.21:51468") d.request_signal("franka_lidar", pab.MsgType.franka_lidar) def gen_lidarstats(broker, n_samples, n_lidars=9): print('collecting signal values') list_msmt = [] for i in range(n_samples): msmt = broker.recv_msg("franka_lidar", -1).get_data() list_msmt.append(msmt) if i % 10 == 0: print(i) list_msmt = np.array(list_msmt) means = list_msmt.mean(axis=0) stds = list_msmt.std(axis=0) maxs = list_msmt.max(axis=0) mins = list_msmt.min(axis=0) print('plotting lidar stats') plt.errorbar(np.arange(n_lidars), means, stds, fmt='ok', lw=3) plt.errorbar(np.arange(n_lidars), means, [means - mins, maxs - means], fmt='.k', ecolor='gray', lw=1)
[ dcc.Dropdown(options=JOINT_OPTIONS, value="{}".format(id), id="dropdown-joint-{}".format(id)), dcc.Graph(id=JOINT_ID_TEMP.format(id)), ], style={"width": 550, "display": "inline-block", "margin-left": 20, "margin-right": 20}, ) def lidar_viz(id): return html.Div( [dcc.Graph(id=LIDAR_ID_TEMP.format(id))], style={"width": "80%", "margin-left": "auto", "margin-right": "auto"} ) app = dash.Dash() broker = pab.broker(broker_id) broker.request_signal(REAL_JOINTS_MSG, pab.MsgType.NumpyMsg) broker.request_signal(PRED_JOINTS_MSG, pab.MsgType.NumpyMsg) broker.request_signal(PRED_LIDAR_MSG, pab.MsgType.NumpyMsg) broker.request_signal(REAL_LIDAR_MSG, pab.MsgType.NumpyMsg) # Layout of the whole gui app.layout = html.Div( [ html.H1("Joint Visualizations"), html.Div([joint_viz(i) for i in range(N_STATES_VIZ)]), html.H1("Lidar Visualizations"), html.Div(lidar_viz(0)), dcc.Interval(id="state-interval", interval=STATE_INTERVAL * 1000, n_intervals=0), # in milliseconds html.Div("", id="real-div", style={"display": "none"}), html.Div("", id="real-div-0", style={"display": "none"}),
# this file generates random movements for the robot import dlrc_control as ctrl import numpy as np from pyquaternion import Quaternion import time import py_at_broker as pab #broker = ctrl.initialize() broker = pab.broker("tcp://localhost:51468") broker.register_signal("franka_target_pos", pab.MsgType.target_pos) def gen_msg(broker, fnumber, new_pos, ctrl_mode): msg = pab.target_pos_msg() msg.set_timestamp(time.time()) msg.set_ctrl_t(ctrl_mode) msg.set_fnumber(fnumber) msg.set_pos(new_pos) msg.set_time_to_go(0.2) broker.send_msg("franka_target_pos", msg) ctrl_mode = 2 if ctrl_mode == 0: # moving to specific points f = 0 while (True):
def main(): signal.signal(signal.SIGINT, signal_handler) # Parse arguments argparser = argparse.ArgumentParser() # argparser.add_argument("-n", "--name", # help="Specifies a pickle file name to use") argparser.add_argument("-l", "--lidar", help="Capture lidar data", action="store_true") argparser.add_argument("-r", "--realsense", help="Capture realsense data", action="store_true") args = argparser.parse_args() if args.lidar: print("Recording Lidar data") if args.realsense: print("Recording Realsense data") print("This script will always record state data") # Set up the broker broker = pab.broker("tcp://10.250.144.21:51468") # # in any case, record the robot data broker.request_signal("franka_state", pab.MsgType.franka_state) # do not record the lidar data in simulator if args.lidar: broker.request_signal("franka_lidar", pab.MsgType.franka_lidar) # only record the camera data of requested if args.realsense: broker.request_signal("realsense_images", pab.MsgType.realsense_image) # simply record the data while (True): reading = dict() reading['systemtime'] = datetime.datetime.now() print(f'time: {reading["systemtime"]}\tstate ', end='') print('trying to receive msg') msg = broker.recv_msg("franka_state", -1) print('msg received') states = get_state(msg) for key in states.keys(): reading['state_' + key] = states[key] if args.lidar: print('lidar', end='') msg = broker.recv_msg("franka_lidar", -1) lidars = get_lidar(msg) for key in lidars.keys(): reading['lidar_' + key] = lidars[key] if args.realsense: print('realsense') msg = broker.recv_msg("realsense_images", -1) realsenses = get_realsense(msg) for key in realsenses.keys(): reading['realsense_' + key] = realsenses[key] store.append(reading)
def main(): signal.signal(signal.SIGINT, signal_handler) # Parse arguments argparser = argparse.ArgumentParser() argparser.add_argument("-n", "--name", help="Specifies a pickle file name to use") argparser.add_argument("-s", "--simulator", help="Indicates working with the simulator", action="store_true") argparser.add_argument( "-r", "--realsense", help="Indicates if realsense data should be captured", action="store_true") args = argparser.parse_args() if not args.simulator: print("Working with the REAL ROBOT.\n" "Provide the argument --simulator or -s" " to record simulator data") else: print("Working with the SIMULATOR") # check the file name if not args.name: print("No pickle file added, creating one based on time stamp") # create ISO 8601 UTC time stamped file name fname = datetime.datetime.utcnow()\ .replace(tzinfo=datetime.timezone.utc)\ .replace(microsecond=0).isoformat() fname = "".join([ c for c in fname if c.isalpha() or c.isdigit() or c == ' ' ]).rstrip() if args.simulator: fname = fname + "_SIMULATOR" else: fname = fname + "_REALBOT" fname = fname + ".pkl" elif os.access(os.path.dirname(args.name), os.W_OK): fname = args.name else: raise ValueError("No valid pickle file name given") # Set up the broker broker = pab.broker("tcp://10.250.144.21:51468") # # in any case, record the robot data broker.request_signal("franka_state", pab.MsgType.franka_state) # do not record the lidar data in simulator if not args.simulator: broker.request_signal("franka_lidar", pab.MsgType.franka_lidar) # only record the camera data of requested if args.realsense: print('Recording REALSENSE data') broker.request_signal("realsense_images", pab.MsgType.realsense_image) # simply record the data while (True): reading = dict() reading['systemtime'] = datetime.datetime.now() print(f'time: {reading["systemtime"]}\tstate ', end='') msg = broker.recv_msg("franka_state", -1) states = get_state(msg) for key in states.keys(): reading['state_' + key] = states[key] if not args.simulator: print('lidar', end='') msg = broker.recv_msg("franka_lidar", -1) lidars = get_lidar(msg) for key in lidars.keys(): reading['lidar_' + key] = lidars[key] if args.realsense: print('realsense') msg = broker.recv_msg("realsense_images", -1) realsenses = get_realsense(msg) for key in realsenses.keys(): reading['realsense_' + key] = realsenses[key] store.append(reading)