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)