def publish_height_data(self):
height_data = VectorField()
height_data.child_frame_id = "odom"
for i in range(1,HEIGHT_MAP_DATA_NUM*2-1):
for j in range(1,HEIGHT_MAP_DATA_NUM*2-1):
if self.map[i][j][1] > 0:
tmp_pos = Vector3()
tmp_pos.x = (i - HEIGHT_MAP_DATA_NUM)*HEIGHT_MAP_DATA_GRID_LENGTH + self.center_x
tmp_pos.y = (j - HEIGHT_MAP_DATA_NUM)*HEIGHT_MAP_DATA_GRID_LENGTH + self.center_y
tmp_pos.z = self.map[i][j][0]
height_data.pos.append(tmp_pos)
height_data.data_num = len(height_data.pos)
self.pub_p.publish(height_data)
def publish_gradient(self):
grad_data = VectorField()
grad_data.child_frame_id = "odom"
count = 0
for i in range(1,HEIGHT_MAP_DATA_NUM*2-1):
for j in range(1,HEIGHT_MAP_DATA_NUM*2-1):
absgrad = self.gradient_map[i][j][0]*self.gradient_map[i][j][0] + self.gradient_map[i][j][1]*self.gradient_map[i][j][1]
if (0.0*0.0)<absgrad and absgrad<1000000:
if count % SHOW_RATE == 0:
tmp_pos = Vector3()
tmp_pos.x = (i - HEIGHT_MAP_DATA_NUM)*HEIGHT_MAP_DATA_GRID_LENGTH + self.center_x
tmp_pos.y = (j - HEIGHT_MAP_DATA_NUM)*HEIGHT_MAP_DATA_GRID_LENGTH + self.center_y
tmp_pos.z = 0
tmp_vec = Vector3()
tmp_vec.x = self.gradient_map[i][j][0]
tmp_vec.y = self.gradient_map[i][j][1]
tmp_vec.z = 0
grad_data.pos.append(tmp_pos)
grad_data.vec.append(tmp_vec)
count += 1
grad_data.data_num = len(grad_data.pos)
self.pub.publish(grad_data)
