MAP_SIZE = np.array((ARRAY_SIZE - 1) / 2)
# mapping data
cost_map = np.ones((ARRAY_SIZE), dtype=float) * INF
cost_map[START_Y, START_X] = 0
cost_map_colored = np.zeros((ARRAY_SIZE), dtype=int)
# node queue
nodes = [Node([START_Y, START_X])]
DIRECTIONS = np.array([[0, 1], [1, 0], [0, -1], [-1, 0]]) #右,上,左,下の４方向
#DIRECTIONS = np.array([[ 0, 1], [ 1, 1], [ 1,0], [ 1,-1], [ 0,-1], [-1,-1], [-1, 0], [-1, 1]]) #８方向用
cell_matrix = ARRAY_SIZE
cell_size = [0.200, 0.200]
path1 = path.pathPublisher("path_green", cell_size)
walls = wall.wallPublisher("wall_marker", cell_matrix, cell_size)
heat = heatmap.heatmapPublisher("heatmap", cell_matrix, cell_size)
walls.setData(maze)
try:
loop = rospy.Rate(10) # 10[loop/s]の設定をする。
while not (rospy.is_shutdown()):
#------generate cost map-----
while not (len(nodes) == 0):
print("--------------------------------------------")
for node in nodes:
for dire in DIRECTIONS:
(ny, nx) = node.pos
(search_y, search_x) = node.pos + dire
if (0 <= search_x and search_x < ARRAY_SIZE[1]) and (
0 <= search_y and search_y < ARRAY_SIZE[0]):
cost = (calc_cost(ny, nx, search_y, search_x) +
import roslib.packages
pk_path = roslib.packages.get_pkg_dir('micro_mouse')
import sys
sys.path.append(pk_path + '/script/lib')
import math
import numpy as np
import rospy # include<ros/ros.h>のようなもの
import heatmap
if __name__ == '__main__': # int main()みたいな
rospy.init_node("node_name") # ノード設定
cell_matrix = [10, 10]
cell_size = [0.200, 0.200]
heatmap = heatmap.heatmapPublisher("heatmap_red", cell_matrix, cell_size)
map_data = np.arange(100).reshape(10, 10)
try:
loop = rospy.Rate(10) # 10[loop/s]の設定をする。
while not rospy.is_shutdown():
#-----ここにプログラムを書く-----
heatmap.setData(map_data, heatmap.RED, 100)
#-----地図情報を表示する-----
heatmap.publish()
#-----ここまで-----
loop.sleep() # 10[loop/s]になるよう調整する。
except KeyboardInterrupt: # try:中にCTRL-Cが押されればココを実行する。
print("キーボード割り込み！CTRL-C 終了")