class RectanglePixelSlit(_RectangleSlitBase):
"""
Slit constructed using WCS and coords information.
Utilizes `regions` package.
Parameters
----------
x, y : float
Center (x, y) of slit in pix.
width, length : float
width and length of slit in pix.
"""
def __init__(self,
x=None,
y=None,
width=None,
length=None,
*args,
**kwargs):
pixcoord = PixCoord(x, y)
self._pix_region = RectanglePixelRegion(center=pixcoord,
width=width,
height=length)
self._patch = self._pix_region.as_artist(edgecolor='red',
facecolor='none')
self._is_active = False
class RectanglePixelSlit(_RectangleSlitBase):
"""
Slit constructed using WCS and coords information.
Utilizes `regions` package.
Parameters
----------
x, y : float
Center (x, y) of slit in pix.
width, length : float
width and length of slit in pix.
"""
def __init__(self, x=None, y=None, width=None, length=None, *args, **kwargs):
pixcoord = PixCoord(x, y)
self._pix_region = RectanglePixelRegion(center=pixcoord, width=width, height=length)
self._patch = self._pix_region.as_artist(edgecolor='red', facecolor='none')
self._is_active = False
def plot_one_box(box, img, color=None, label=None, line_thickness=None):
# Plots one bounding box on image img
tl = line_thickness or round(
0.002 * (img.shape[0] + img.shape[1]) / 2) + 1 # line/font thickness
color = color or [random.randint(0, 255) for _ in range(3)]
x, y = int(box[0]), int(box[1])
width, height = int(box[2]), int(box[3])
angle = Angle(-int(180 * box[4] / np.pi), 'deg')
center = PixCoord(x=x, y=y)
reg = RectanglePixelRegion(center=center,
width=width,
height=height,
angle=angle)
patch = reg.as_artist()
path = patch.get_path()
vertices = path.vertices.copy()
vertices = patch.get_patch_transform().transform(vertices)
cv2.drawContours(img, [vertices.astype(int)], -1, color, tl)
"""
def odom_callback(self, msg):
global switch
global euler_z
global button_1, button_3, button_4, button_5
pos_x = msg.pose.pose.position.x
pos_y = msg.pose.pose.position.y
pos_z = msg.pose.pose.position.z
ori_x = msg.pose.pose.orientation.x
ori_y = msg.pose.pose.orientation.y
ori_z = msg.pose.pose.orientation.z
ori_w = msg.pose.pose.orientation.w
if (abs(pos_y) > self.deviation):
self.deviation = abs(pos_y)
euler_x, euler_y, euler_z = self.quaternion_to_euler_angle(ori_x, ori_y, ori_z, ori_w)
if self.graph_switch == True:
plt.rcParams["figure.figsize"] = [9,9]
plt.rcParams['legend.numpoints'] = 1
plt.grid(alpha=0.2, color='k', linestyle='-.', dash_capstyle='butt', drawstyle='steps', linewidth=0.5)
if switch == 0:
plt.title('Obstacle Avoidance', loc='center')
plt.xlabel('x-axis')
plt.ylabel('y-axis')
# ROBOT POSITION
rp_x, rp_y, rp_z = vrep.simxGetObjectPosition(self.clientID, self.robot_handle, -1, vrep.simx_opmode_streaming)[1]
# DYNAMIC OBSTACLE
d_x, d_y, d_z = vrep.simxGetObjectPosition(self.clientID, self.d_obs_handle, -1, vrep.simx_opmode_streaming)[1]
scale = self.calc_dis(d_x, d_y, rp_x, rp_y)
#print(self.gradient(scale))
#print(scale)
if scale <= 10:
self.alpha = scale / 10
else:
self.alpha = 1.0
# calculate distance between robot and obstacle
if (scale >= 1):
color = [ str(self.gradient(scale)) ]
_color = (self.gradient(scale)/255, self.gradient(scale)/255, self.gradient(scale)/255)
else:
color = [255]
_color = (1, 1, 1)
if (d_x != 0 and d_y != 0):
# # 장애물과 로봇 사이의 거리
# dis = self.calc_dis(rp_x, rp_y, 4.95, d_y)
# if (dis <= 1.1):
# coll_check1 = 'r'
# coll_check2 = (1,0,0)
# else:
# coll_check1 = 'k'
# coll_check2 = (0,0,0)
# DYNAMIC OBSTACLE
#plt.scatter(d_y, d_x, marker='o', s=300, edgecolors='k', facecolor='none', label='dynamic obstacle')
# [1] DYNAMIC OBSTACLE SHAPE
#plt.scatter(-d_y, d_x, marker='o', s=600, edgecolors=coll_check1, facecolor=_color, label='dynamic obstacle')
# circle = plt.Circle((-d_y, d_x), 0.5, edgecolor=coll_check1, facecolor=_color)
# plt.gca().add_patch(circle)
# self.csv_container[3] = -d_y
# self.csv_container[4] = d_x
# # [2] DYNAMIC OBSTACLE TRACE
# if (d_y > self._d_y):
# dynamic_obstacle, = plt.plot( [-self._d_y, -d_y], [4.95, 4.95], marker=(3, 0, 90), markersize = 5, linewidth=1.0, color='b', label='dynamic obstacle')
# else:
# dynamic_obstacle, = plt.plot( [-self._d_y, -d_y], [4.95, 4.95], marker=(3, 0, -90), markersize = 5, linewidth=1.0, color='b', label='dynamic obstacle')
# self._d_y = d_y
# self._d_x = d_x
# ROBOT POSITION WITH TRACE
x, y = -rp_y, rp_x #-pos_y, pos_x
width, height = 0.6, 0.83
angle = Angle(euler_z, 'deg') #Angle(euler_z, 'deg')
center = PixCoord(x=x, y=y)
reg = RectanglePixelRegion(center=center, width=width,
height=height, angle=angle)
patch = reg.as_artist(facecolor=_color, edgecolor='k', lw=1.0, fill=True) #, label='Holonomic Robot')
plt.gca().add_patch(patch)
# [2] ROBOT CENTER & TRACE
robot_footprint, = plt.plot( [-self._pos_y, -rp_y], [self._pos_x, rp_x], marker=(3, 0, euler_z), markersize = 5, linewidth=1.0, color='r', label='ODG-PF')
self._pos_y = rp_y #pos_y
self._pos_x = rp_x #pos_x
dis1 = self.calc_dis(-self.x_rand1, 2.6, -rp_y, rp_x)
dis3 = self.calc_dis(-self.x_rand3, -0.6, -rp_y, rp_x)
dis4 = self.calc_dis(-self.x_rand4, -2.95, -rp_y, rp_x)
dis5 = self.calc_dis(-self.x_rand5, -5.8, -rp_y, rp_x)
# obs 1
if (dis1 <= 1.2):
coll_check_1_1 = 'r'
coll_check_1_2 = (1,0,0)
button_1 = True
else:
coll_check_1_1 = 'k'
coll_check_1_2 = (0,0,0)
button_1 = False
# obs 3
if (dis3 <= 1.2):
coll_check_3_1 = 'r'
coll_check_3_2 = (1,0,0)
button_3 = True
else:
coll_check_3_1 = 'k'
coll_check_3_2 = (0,0,0)
button_3 = False
# obs 4
if (dis4 <= 1.2):
coll_check_4_1 = 'r'
coll_check_4_2 = (1,0,0)
button_4 = True
else:
coll_check_4_1 = 'k'
coll_check_4_2 = (0,0,0)
button_4 = False
# obs 5
if (dis5 <= 1.2):
coll_check_5_1 = 'r'
coll_check_5_2 = (1,0,0)
button_5 = True
else:
coll_check_5_1 = 'k'
coll_check_5_2 = (0,0,0)
button_5 = False
# STATIC OBSTACLE 1
if button_1 == False:
center = PixCoord(x=-self.x_rand1, y = 6.75)
reg = RectanglePixelRegion(center=center, width=1,
height=1)
patch = reg.as_artist(facecolor='k', edgecolor='none', lw=1.0, fill=True)
plt.gca().add_patch(patch)
button_1 = True
else:
center = PixCoord(x=-self.x_rand1, y=6.75)
reg = RectanglePixelRegion(center=center, width=1,
height=1)
patch = reg.as_artist(facecolor='k', edgecolor='none', lw=1.0, fill=True)
plt.gca().add_patch(patch)
# STATIC OBSTACLE 3
if button_3 == False:
center = PixCoord(x=-self.x_rand3, y=1.75)
reg = RectanglePixelRegion(center=center, width=1,
height=1)
patch = reg.as_artist(facecolor='k', edgecolor='none', lw=1.0, fill=True)
plt.gca().add_patch(patch)
button_3 = True
else:
center = PixCoord(x=-self.x_rand3, y=1.75)
reg = RectanglePixelRegion(center=center, width=1,
height=1)
patch = reg.as_artist(facecolor='k', edgecolor=coll_check_3_1, lw=1.0, fill=True)
plt.gca().add_patch(patch)
# STATIC OBSTACLE 4
if button_4 == False:
center = PixCoord(x=-self.x_rand4, y=-2.75)
reg = RectanglePixelRegion(center=center, width=1,
height=1)
patch = reg.as_artist(facecolor='k', edgecolor='none', lw=1.0, fill=True)
plt.gca().add_patch(patch)
button_4 = True
else:
center = PixCoord(x=-self.x_rand4, y=-2.75)
reg = RectanglePixelRegion(center=center, width=1,
height=1)
patch = reg.as_artist(facecolor='k', edgecolor=coll_check_4_1, lw=1.0, fill=True)
plt.gca().add_patch(patch)
# STATIC OBSTACLE 5
if button_5 == False:
center = PixCoord(x=-self.x_rand5, y=-5.75)
reg = RectanglePixelRegion(center=center, width=1,
height=1)
patch = reg.as_artist(facecolor='k', edgecolor='none', lw=1.0, fill=True)
plt.gca().add_patch(patch)
button_5 = True
else:
center = PixCoord(x=-self.x_rand5, y=-5.75)
reg = RectanglePixelRegion(center=center, width=1,
height=1)
patch = reg.as_artist(facecolor='k', edgecolor=coll_check_5_1, lw=1.0, fill=True)
plt.gca().add_patch(patch)
#plt.grid()
plt.xticks(np.arange(-10, 11, 1))
plt.yticks(np.arange(-10, 11, 1))
plt.draw()
if switch < 2:
try:
static_obstacle = mpatches.Patch(hatch='', color='black', label="static obstacle")
plt.legend(handles=[static_obstacle, dynamic_obstacle, robot_footprint])
switch += 1
except:
static_obstacle = mpatches.Patch(hatch='', color='black', label="static obstacle")
plt.legend(handles=[static_obstacle])
plt.pause(1)
goal_dis = self.calc_dis(pos_x, pos_y, 9, 0) # V-REP 축이 뒤틀렸음
simulationTime=vrep.simxGetLastCmdTime(self.clientID) / 1000 # sec로 단위 변환
# f.close()
if (goal_dis < 1.2 or simulationTime > 90):
if (goal_dis) < 1.2:
x, y = -pos_y, pos_x
width, height = 0.6, 0.83
angle = Angle(euler_z, 'euler') # 원래는 Angle(euler_z, 'euler')였는데, 이렇게 바꾸면 인터넷 연결이 필요하던 Angle이 필요가 없어지게됨
center = PixCoord(x=x, y=y)
reg = RectanglePixelRegion(center=center, width=width,
height=height, angle=angle)
patch = reg.as_artist(facecolor=_color, edgecolor='m', lw=1.5, fill=True, label='Goal Arrived')
plt.gca().add_patch(patch)
print("[INFO] EPISODE DONE")
plt.savefig('/home/yu-bin/ODMGPF_EXPERIMENT/OD-MGPF_{}_{}_{}_{:.3f}.png'.format(d_threshold, gamma, k_holo, self.deviation, dpi = 300))
# RESULT TO CSV
result = {'RESULT': [str(d_threshold)+" "+str(gamma)+" "+str(k_holo)], 'DEVIATION': [str(self.deviation)]}
df = pd.DataFrame(result)
if not os.path.exists('/home/yu-bin/ODMGPF_EXPERIMENT/output.csv'):
df.to_csv('/home/yu-bin/ODMGPF_EXPERIMENT/output.csv', index=False, mode='w', encoding='utf-8-sig')
else:
df.to_csv('/home/yu-bin/ODMGPF_EXPERIMENT/output.csv', index=False, mode='a', encoding='utf-8-sig', header=False)
rospy.signal_shutdown(self.myhook())
def plot_labels(labels, save_dir=Path(''), loggers=None):
# plot dataset labels
print('Plotting labels... ')
c, b = labels[:, 0], labels[:, 1:].transpose() # classes, boxes
nc = int(c.max() + 1) # number of classes
colors = color_list()
x = pd.DataFrame(b.transpose(),
columns=['x', 'y', 'width', 'height', 'angle'])
# seaborn correlogram
sns.pairplot(x,
corner=True,
diag_kind='auto',
kind='hist',
diag_kws=dict(bins=50),
plot_kws=dict(pmax=0.9))
plt.savefig(save_dir / 'labels_correlogram.jpg', dpi=200)
plt.close()
# matplotlib labels
matplotlib.use('svg') # faster
ax = plt.subplots(5, 1, figsize=(4, 20), tight_layout=True)[1].ravel()
ax[0].hist(c, bins=np.linspace(0, nc, nc + 1) - 0.5, rwidth=0.8)
ax[0].set_xlabel('classes')
sns.histplot(x, x='x', y='y', ax=ax[2], bins=50, pmax=0.9)
sns.histplot(x, x='width', y='height', ax=ax[3], bins=50, pmax=0.9)
sns.histplot(x, x='angle', ax=ax[4], bins=50, pmax=0.9)
# rectangles
labels[:, 1:3] = 0.5 # center
labels[:, 1:5] = labels[:, 1:5] * 2000
img = Image.fromarray(np.ones((2000, 2000, 3), dtype=np.uint8) * 255)
for cls, *box in labels[:1000]:
x, y = int(box[0]), int(box[1])
width, height = int(box[2]), int(box[3])
angle = Angle(-int(180 * box[4] / np.pi), 'deg')
center = PixCoord(x=x, y=y)
reg = RectanglePixelRegion(center=center,
width=width,
height=height,
angle=angle)
patch = reg.as_artist()
path = patch.get_path()
vertices = path.vertices.copy()
vertices = patch.get_patch_transform().transform(vertices)
vertices = vertices.astype(int)
vertices = tuple(i for i in zip(*vertices.T))
ImageDraw.Draw(img).polygon(vertices,
outline=colors[int(cls) % 10]) # plot
ax[1].imshow(img)
ax[1].axis('off')
for a in [0, 1, 2, 3, 4]:
for s in ['top', 'right', 'left', 'bottom']:
ax[a].spines[s].set_visible(False)
plt.savefig(save_dir / 'labels.jpg', dpi=200)
matplotlib.use('Agg')
plt.close()
# loggers
for k, v in loggers.items() or {}:
if k == 'wandb' and v:
v.log({
"Labels": [
v.Image(str(x), caption=x.name)
for x in save_dir.glob('*labels*.jpg')
]
})