def update(self):
self.get_logger().info('on update')
x_star = self.controller.calc_next_point() # Loop 1) 2) 3)
# Wait for response
response = self.request_service_sync(
self.cli_target_object,
encode_array(array=x_star.x, comm_data=self.req))
# Do something about response
y_star = decode_array(response)
self.controller.add_x_y_to_T(x_star.x, y_star) # Loop 5)
def obtain_y_at_x(self, x):
self.req = encode_array(array=x, comm_data=self.req)
self.future = self.cli_target_object.call_async(self.req)
while True:
if not self.isSetController:
rclpy.spin_once(self)
if self.future.done():
try:
response = self.future.result()
except Exception as e:
self.get_logger().info('Service call failed %r' % (e, ))
else:
self.get_logger().info('Success!')
break
y = decode_array(response)
return y
def update(self):
self.get_logger().info('on update')
# 1) 2) 3) in Loop
x_star = self.controller.calc_next_point()
# 4) in Loop # Wait for response
response = self.request_service_sync(
self.cli_target_object,
encode_array(array=x_star.x, comm_data=self.req))
# 5) in Loop
y_star = decode_array(response)
self.controller.add_x_y_to_T(x_star.x, y_star)
self.iter_num += 1
def draw_gp_result(self, msg):
if not msg.iter_num > self.iter_num:
if self.image_msg is None:
pass
else:
self.pub_gp_result.publish(self.image_msg)
self.get_logger().info("Published Image")
return
self.iter_num = msg.iter_num
x_sampling_points = decode_array(msg.x_sampling_points).T
mean = decode_array(msg.mean).T
var = decode_array(msg.var).T
train_x = decode_array(msg.train_x).T
train_y = decode_array(msg.train_y).T
# Figure & Axes Setting
fig = plt.figure()
ax = fig.add_subplot(111)
ax.grid()
ax.set_xlabel("x")
ax.set_ylabel("y")
ax.set_xlim(
np.min(x_sampling_points) - 1,
np.max(x_sampling_points) + 1,
)
ax.set_ylim(-2, 2)
# Draw Graph
for x_sampling_points_, mean_, var_, train_x_, train_y_ in zip(
x_sampling_points, mean, var, train_x, train_y):
plt.plot(x_sampling_points_, mean_)
ax.fill_between(
x_sampling_points_, #TODO: 一次元しか対応していない
mean_ - 3 * var_, # 3σを採用
mean_ + 3 * var_,
facecolor='blue',
alpha=0.5)
plt.scatter(train_x_[:-1], train_y_[:-1], c='blue')
plt.axvline(x=train_x_[-1], c='red')
if self.iter_num > 2:
plt.scatter(train_x_[-2], train_y_[-2], c='green')
# Transform plt to numpy
buf = io.BytesIO() # bufferを用意
plt.savefig(buf, format='png') # bufferに保持
enc = np.frombuffer(buf.getvalue(), dtype=np.uint8) # bufferからの読み出し
dst = cv2.imdecode(enc, 1) # デコード
# dst = dst[:, :, ::-1] # BGR->RGB
buf.close()
plt.close()
# Publish Image
bridge = CvBridge()
self.image_msg = bridge.cv2_to_imgmsg(dst, encoding="bgr8")
try:
self.pub_gp_result.publish(self.image_msg)
except CvBridgeError as e:
print(e)
self.get_logger().info("Published Image")
def surface_shape_callback(self, request, response):
x = decode_array(request)
surface_array = self.target_object.surface_shape(x)
# surface_shape's type is np.array, so it should be transform to list (not nest)
response = encode_array(surface_array, response)
return response