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)
